From 7e55c46a277f27fe2feaf5a2f6e66596ef6dedf2 Mon Sep 17 00:00:00 2001 From: Luke Maxwell <91603936+lmaxwell24@users.noreply.github.com> Date: Thu, 2 Mar 2023 17:58:26 -0500 Subject: [PATCH] Pathplanning and autonomous (#27) * use pathplannerlib with markers * in lab paths * add requirement * documentation, indexed commands in marker map * flip based on alliance * log auto, proper fliparound * ok actually proper fliparound with respect to world coords * more logging in autos * rotate controls based off of which alliance side * log auto error * more logging * new auton paths with waypoints * wrap commands to prevent errors * deal with conflicting commands --- .pathplanner/settings.json | 7 + commands/auto/autoeventnames.md | 15 ++ commands/auto/autohelper.py | 14 ++ commands/auto/autonomousaction.py | 117 ++++++++++ commands/auto/followtrajectory.py | 213 ++++++++++++++++++ commands/drive/absoluterelativedrive.py | 35 ++- commands/followtrajectory.py | 52 ----- commands/trajectoryauto.py | 34 --- constants.py | 12 +- deploy/pathplanner/1 Cable Run.path | 55 +++++ deploy/pathplanner/1 Loading Station.path | 53 +++++ deploy/pathplanner/1 Middle Engaged.path | 55 +++++ deploy/pathplanner/2 Cable Run Engage.path | 141 ++++++++++++ deploy/pathplanner/2 Cable Run.path | 139 ++++++++++++ .../pathplanner/2 Loading Station Engage.path | 142 ++++++++++++ deploy/pathplanner/2 Loading Station.path | 151 +++++++++++++ deploy/pathplanner/2 Middle Engaged.path | 175 ++++++++++++++ deploy/pathplanner/Test Path.path | 106 +++++++++ networktables.json | 16 ++ requirements.txt | 1 + robotcontainer.py | 11 +- 21 files changed, 1441 insertions(+), 103 deletions(-) create mode 100644 .pathplanner/settings.json create mode 100644 commands/auto/autoeventnames.md create mode 100644 commands/auto/autohelper.py create mode 100644 commands/auto/autonomousaction.py create mode 100644 commands/auto/followtrajectory.py delete mode 100644 commands/followtrajectory.py delete mode 100644 commands/trajectoryauto.py create mode 100644 deploy/pathplanner/1 Cable Run.path create mode 100644 deploy/pathplanner/1 Loading Station.path create mode 100644 deploy/pathplanner/1 Middle Engaged.path create mode 100644 deploy/pathplanner/2 Cable Run Engage.path create mode 100644 deploy/pathplanner/2 Cable Run.path create mode 100644 deploy/pathplanner/2 Loading Station Engage.path create mode 100644 deploy/pathplanner/2 Loading Station.path create mode 100644 deploy/pathplanner/2 Middle Engaged.path create mode 100644 deploy/pathplanner/Test Path.path diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json new file mode 100644 index 0000000..c07b76c --- /dev/null +++ b/.pathplanner/settings.json @@ -0,0 +1,7 @@ +{ + "robotWidth": 0.83, + "robotLength": 0.78, + "holonomicMode": true, + "generateJSON": false, + "generateCSV": false +} \ No newline at end of file diff --git a/commands/auto/autoeventnames.md b/commands/auto/autoeventnames.md new file mode 100644 index 0000000..5c706ed --- /dev/null +++ b/commands/auto/autoeventnames.md @@ -0,0 +1,15 @@ +# Autonomous Event Naming + +### because markers need names + +#### namings that become actual positions + +| Name | Action | +| ------- | --------------------------------------------------------------------------------------- | +| store | stores the arm inwards | +| top | puts the arm into the TOP score position | +| mid | puts the arm into the MID score position | +| hybrid | puts the arm into the HYBRID score position | +| engage | auto balance and engage with the charging station (continuous, command does not finish) | +| intake | runs the collection mechanism to intake a gamepiece | +| outtake | runs the collection mechanism to outtake a gamepiece | diff --git a/commands/auto/autohelper.py b/commands/auto/autohelper.py new file mode 100644 index 0000000..1997511 --- /dev/null +++ b/commands/auto/autohelper.py @@ -0,0 +1,14 @@ +from typing import List +from pathplannerlib import PathPlanner, PathPlannerTrajectory +from wpilib import DataLogManager + +import constants + + +def trajectoryFromFile(name: str) -> List[PathPlannerTrajectory]: + DataLogManager.log(f"Loading path {name}") + return PathPlanner.loadPathGroup( + name, + constants.kMaxForwardLinearVelocity, + constants.kMaxForwardLinearAcceleration, + ) diff --git a/commands/auto/autonomousaction.py b/commands/auto/autonomousaction.py new file mode 100644 index 0000000..a536b00 --- /dev/null +++ b/commands/auto/autonomousaction.py @@ -0,0 +1,117 @@ +from typing import Dict, List +from commands2 import ( + Command, + CommandBase, + FunctionalCommand, + ParallelCommandGroup, + ParallelDeadlineGroup, + SequentialCommandGroup, + WaitCommand, +) +from pathplannerlib import PathPlannerTrajectory +from wpilib import DataLogManager + +from commands.auto.autohelper import trajectoryFromFile +from commands.auto.followtrajectory import FollowTrajectory +from commands.resetdrive import ResetDrive +from subsystems.drivesubsystem import DriveSubsystem + + +class AutonomousRoutine(SequentialCommandGroup): + markerMap: Dict[str, Command] + + def __init__( + self, + drive: DriveSubsystem, + name: str, + simultaneousCommands: List[Command], + ): + self.name = name + self.markerMap = { # later todo: actual implementation + "store": WaitCommand(2), + "top": WaitCommand(2), + "mid": WaitCommand(2), + "hybrid": WaitCommand(2), + "engage": WaitCommand(5), + "intake": WaitCommand(0.25), + "outtake": WaitCommand(0.25), + } + self.paths = trajectoryFromFile(name) + followCommands = [ + SequentialCommandGroup( + self.stopEventGroup(path.getStartStopEvent()), + FollowTrajectory(drive, path, path.getMarkers(), self.markerMap), + self.stopEventGroup(path.getEndStopEvent()), + ) + for path in self.paths + ] + + super().__init__( + ResetDrive(drive), + ParallelCommandGroup( + SequentialCommandGroup(*followCommands), *simultaneousCommands + ), + ) + self.setName(name) + + def execute(self) -> None: + DataLogManager.log(f"Starting auto: {self.name}") + return super().execute() + + def wrappedEventCommand(self, eventCommand: Command) -> CommandBase: + return FunctionalCommand( + eventCommand.initialize, + eventCommand.execute, + eventCommand.end, + eventCommand.isFinished, + list(eventCommand.getRequirements()), + ) + + def getStopEventCommands( + self, stopEvent: PathPlannerTrajectory.StopEvent + ) -> Command: + commands = [ + self.wrappedEventCommand(self.markerMap.get(name)) + for name in stopEvent.names + if name in self.markerMap + ] + if ( + stopEvent.executionBehavior + == PathPlannerTrajectory.StopEvent.ExecutionBehavior.PARALLEL + ): + return ParallelCommandGroup(commands) + elif ( + stopEvent.executionBehavior + == PathPlannerTrajectory.StopEvent.ExecutionBehavior.SEQUENTIAL + ): + return SequentialCommandGroup(commands) + else: + raise NotImplementedError("That execution behaviour is not implemented") + + def stopEventGroup(self, stopEvent: PathPlannerTrajectory.StopEvent) -> Command: + eventCommands = self.getStopEventCommands(stopEvent) + if ( + stopEvent.waitBehavior + == PathPlannerTrajectory.StopEvent.WaitBehavior.BEFORE + ): + return SequentialCommandGroup( + WaitCommand(stopEvent.waitTime), eventCommands + ) + elif ( + stopEvent.waitBehavior == PathPlannerTrajectory.StopEvent.WaitBehavior.AFTER + ): + return SequentialCommandGroup( + eventCommands, WaitCommand(stopEvent.waitTime) + ) + elif ( + stopEvent.waitBehavior + == PathPlannerTrajectory.StopEvent.WaitBehavior.DEADLINE + ): + return ParallelDeadlineGroup(WaitCommand(stopEvent.waitTime), eventCommands) + elif ( + stopEvent.waitBehavior + == PathPlannerTrajectory.StopEvent.WaitBehavior.MINIMUM + ): + return ParallelCommandGroup(WaitCommand(stopEvent.waitTime), eventCommands) + else: + return eventCommands diff --git a/commands/auto/followtrajectory.py b/commands/auto/followtrajectory.py new file mode 100644 index 0000000..6a2a44a --- /dev/null +++ b/commands/auto/followtrajectory.py @@ -0,0 +1,213 @@ +from math import pi +import functools +import operator +from typing import Dict, List, Tuple +from commands2 import Command, CommandBase +from pathplannerlib import PathPlannerTrajectory +from wpilib import DataLogManager, DriverStation, SmartDashboard, Timer +from wpimath.controller import ( + PIDController, + ProfiledPIDControllerRadians, + HolonomicDriveController, +) +from wpimath.geometry import Pose2d, Rotation2d +from wpimath.trajectory import TrapezoidProfileRadians + +from subsystems.drivesubsystem import DriveSubsystem +import constants + + +class FollowTrajectory(CommandBase): + unpassedMarkers: List[PathPlannerTrajectory.EventMarker] + + def __init__( + self, + drive: DriveSubsystem, + trajectory: PathPlannerTrajectory, + markers: List[PathPlannerTrajectory.EventMarker], + markerMap: Dict[str, Command], + ) -> None: + CommandBase.__init__(self) + + self.drive = drive + self.markers = markers + self.markerMap = markerMap + + self.currentCommands: List[Tuple[Command, bool]] = [] + + for marker in self.markers: + for markerName in marker.names: + if markerName in self.markerMap.keys(): + reqs = self.markerMap[markerName].getRequirements() + self.addRequirements( + reqs + ) # make sure we require everything our sub commands require + + self.setControllers() + + self.originTrajectory = trajectory + self.trajectory = trajectory + + self.timer = Timer() + + self.addRequirements([self.drive]) + self.setName(__class__.__name__) + + def setControllers(self) -> None: + self.xController = PIDController( + constants.kTrajectoryPositionPGain, + constants.kTrajectoryPositionIGain, + constants.kTrajectoryPositionDGain, + ) + self.yController = PIDController( + constants.kTrajectoryPositionPGain, + constants.kTrajectoryPositionIGain, + constants.kTrajectoryPositionDGain, + ) + self.thetaController = ProfiledPIDControllerRadians( + constants.kTrajectoryAnglePGain, + constants.kTrajectoryAngleIGain, + constants.kTrajectoryAngleDGain, + TrapezoidProfileRadians.Constraints( + constants.kMaxRotationAngularVelocity, + constants.kMaxRotationAngularAcceleration, + ), + ) + self.thetaController.enableContinuousInput(-pi, pi) + + self.controller = HolonomicDriveController( + self.xController, self.yController, self.thetaController + ) + + def initialize(self): + self.timer.reset() + self.timer.start() + + self.unpassedMarkers = [] + self.unpassedMarkers.extend(self.markers) + + initialState = self.trajectory.getInitialState() + allianceRespectiveStartingPoint = self.getAllianceRespectivePoint( + initialState.pose, initialState.holonomicRotation + ) + self.drive.resetOdometryAtPosition( + Pose2d( + allianceRespectiveStartingPoint[0].translation(), + allianceRespectiveStartingPoint[1], + ), + ) + + self.setControllers() + + transformedStates = [ + self.getAllianceRespectivePoint(state.pose, state.pose.rotation()) + for state in self.originTrajectory.getStates() + ] + + SmartDashboard.putNumberArray( + constants.kAutonomousPathKey, + functools.reduce( + operator.add, + [ + [state[0].X(), state[0].Y(), state[1].radians()] + for state in transformedStates + ], + [], + ), + ) + DataLogManager.log("begin trajectory") + + def getAllianceRespectivePoint( + self, pose: Pose2d, holonomicRotation: Rotation2d + ) -> Tuple[Pose2d, Rotation2d]: + if DriverStation.getAlliance() == DriverStation.Alliance.kRed: + return ( + Pose2d( + constants.kFieldLength - pose.X(), + pose.Y(), + -pose.rotation() + Rotation2d(pi), + ), + -holonomicRotation + Rotation2d(pi), + ) + else: + return (pose, holonomicRotation) + + def execute(self) -> None: + for command, running in self.currentCommands: + if not running: + continue + command.execute() + + if command.isFinished(): + command.end(False) + running = False + + curTime = self.timer.get() + currentState = self.drive.getPose() + desiredState = self.trajectory.sample(curTime) + + allianceRespectiveDesiredState = self.getAllianceRespectivePoint( + desiredState.pose, desiredState.holonomicRotation + ) + + SmartDashboard.putNumberArray( + constants.kAutonomousPathError, + [ + currentState.X() - allianceRespectiveDesiredState[0].X(), + currentState.Y() - allianceRespectiveDesiredState[0].Y(), + (currentState.rotation() - allianceRespectiveDesiredState[1]).radians(), + ], + ) + SmartDashboard.putNumberArray( + constants.kAutonomousPathSample, + [ + allianceRespectiveDesiredState[0].X(), + allianceRespectiveDesiredState[0].Y(), + allianceRespectiveDesiredState[1].radians(), + ], + ) + + targetChassisSpeeds = self.controller.calculate( + currentState, + allianceRespectiveDesiredState[0], + desiredState.velocity, + allianceRespectiveDesiredState[1], + ) + + SmartDashboard.putNumberArray( + constants.kAutonomousChassisSpeeds, + [targetChassisSpeeds.vx, targetChassisSpeeds.vy, targetChassisSpeeds.omega], + ) + + self.drive.arcadeDriveWithSpeeds( + targetChassisSpeeds, DriveSubsystem.CoordinateMode.RobotRelative + ) + + # marker relatev stuff + if len(self.unpassedMarkers) > 0 and curTime >= self.unpassedMarkers[0].time: + marker = self.unpassedMarkers.pop(0) + for name in marker.names: + if name in self.markerMap: + eventCommand = self.markerMap[name] + eventCommand.initialize() + self.currentCommands.append((eventCommand, True)) + + def isFinished(self) -> bool: + return self.timer.hasElapsed(self.trajectory.getTotalTime()) + + def getFinalPosition(self) -> Pose2d: + endState = self.trajectory.getEndState() + finalAllianceRespectivePose = self.getAllianceRespectivePoint( + endState.pose, endState.holonomicRotation + ) + return Pose2d( + finalAllianceRespectivePose[0].translation(), + finalAllianceRespectivePose[1], + ) + + def end(self, _interrupted: bool) -> None: + self.drive.resetOdometryAtPosition(self.getFinalPosition()) + self.drive.arcadeDriveWithFactors( + 0, 0, 0, DriveSubsystem.CoordinateMode.RobotRelative + ) + DataLogManager.log("end trajectory") diff --git a/commands/drive/absoluterelativedrive.py b/commands/drive/absoluterelativedrive.py index 0e8d2ba..e4682f2 100644 --- a/commands/drive/absoluterelativedrive.py +++ b/commands/drive/absoluterelativedrive.py @@ -1,6 +1,7 @@ -from math import atan2 +from math import atan2, pi import typing from commands2 import CommandBase +from wpilib import DriverStation from wpimath.controller import PIDController from wpimath.geometry import Rotation2d from subsystems.drivesubsystem import DriveSubsystem @@ -39,17 +40,29 @@ def rotation(self) -> float: if self.rotationX() == 0 and self.rotationY() == 0: return 0 + if DriverStation.getAlliance() == DriverStation.Alliance.kRed: + targetRotation += pi + + optimizedDirection = optimizeAngle( + self.drive.getRotation(), Rotation2d(targetRotation) + ).radians() return self.rotationPid.calculate( - self.drive.getRotation().radians(), - optimizeAngle( - self.drive.getRotation(), Rotation2d(targetRotation) - ).radians(), + self.drive.getRotation().radians(), optimizedDirection ) def execute(self) -> None: - self.drive.arcadeDriveWithFactors( - self.forward(), - self.sideways(), - self.rotation(), - DriveSubsystem.CoordinateMode.FieldRelative, - ) + if DriverStation.getAlliance() == DriverStation.Alliance.kRed: + # if we're on the other side, switch the controls around + self.drive.arcadeDriveWithFactors( + -self.forward(), + -self.sideways(), + self.rotation(), + DriveSubsystem.CoordinateMode.FieldRelative, + ) + else: + self.drive.arcadeDriveWithFactors( + self.forward(), + self.sideways(), + self.rotation(), + DriveSubsystem.CoordinateMode.FieldRelative, + ) diff --git a/commands/followtrajectory.py b/commands/followtrajectory.py deleted file mode 100644 index 50fabb7..0000000 --- a/commands/followtrajectory.py +++ /dev/null @@ -1,52 +0,0 @@ -from math import pi -from commands2 import Swerve4ControllerCommand -from wpimath.controller import ( - PIDController, - ProfiledPIDControllerRadians, -) -from wpimath.trajectory import Trajectory, TrapezoidProfileRadians - -from subsystems.drivesubsystem import DriveSubsystem -import constants - - -class FollowTrajectory(Swerve4ControllerCommand): - def __init__(self, drive: DriveSubsystem, trajectory: Trajectory) -> None: - self.drive = drive - - self.xController = PIDController( - constants.kTrajectoryPositionPGain, - constants.kTrajectoryPositionIGain, - constants.kTrajectoryPositionDGain, - ) - self.yController = PIDController( - constants.kTrajectoryPositionPGain, - constants.kTrajectoryPositionIGain, - constants.kTrajectoryPositionDGain, - ) - self.thetaController = ProfiledPIDControllerRadians( - constants.kTrajectoryAnglePGain, - constants.kTrajectoryAngleIGain, - constants.kTrajectoryAngleDGain, - TrapezoidProfileRadians.Constraints( - constants.kMaxRotationAngularVelocity, - constants.kMaxRotationAngularAcceleration, - ), - ) - self.thetaController.enableContinuousInput(-pi, pi) - - super().__init__( - trajectory, - self.drive.getPose, - self.drive.kinematics, - self.xController, - self.yController, - self.thetaController, - self.drive.applyStates, - [self.drive], - ) - - def end(self, _interrupted: bool) -> None: - self.drive.arcadeDriveWithFactors( - 0, 0, 0, DriveSubsystem.CoordinateMode.RobotRelative - ) diff --git a/commands/trajectoryauto.py b/commands/trajectoryauto.py deleted file mode 100644 index 169b575..0000000 --- a/commands/trajectoryauto.py +++ /dev/null @@ -1,34 +0,0 @@ -from os import path - -from commands2 import SequentialCommandGroup -from wpimath.trajectory import TrajectoryConfig, TrajectoryUtil -from commands.followtrajectory import FollowTrajectory -from commands.resetgyro import ResetGyro - -from subsystems.drivesubsystem import DriveSubsystem -import constants - - -class TrajectoryAuto(SequentialCommandGroup): - def __init__(self, drive: DriveSubsystem, autoName: str) -> None: - self.drive = drive - trajectoryConfig = TrajectoryConfig( - constants.kMaxForwardLinearVelocity, constants.kMaxForwardLinearAcceleration - ) - trajectoryConfig.setKinematics(self.drive.kinematics) - - trajectory = TrajectoryUtil.fromPathweaverJson( - path.join( - path.dirname(path.realpath(__file__)), - "..", - "deploy", - "pathplanner", - "generatedJSON", - autoName + ".wpilib.json", - ) - ) - - super().__init__( - ResetGyro(self.drive, trajectory.sample(0).pose), - FollowTrajectory(self.drive, trajectory), - ) diff --git a/constants.py b/constants.py index 20cdbf1..b9035a6 100644 --- a/constants.py +++ b/constants.py @@ -79,10 +79,10 @@ """seconds""" # Field Physical parameters -kFieldLength = 52.5 * kMetersPerFoot +kFieldLength = 54 * kMetersPerFoot + 3.25 * kMetersPerInch """meters""" -kFieldWidth = 27 * kMetersPerFoot +kFieldWidth = 26 * kMetersPerFoot + 3.5 * kMetersPerInch """meters""" # Robot Physical parameters @@ -470,11 +470,11 @@ """radians / second""" # Trajectory Following -kTrajectoryPositionPGain = 2.0 +kTrajectoryPositionPGain = 4 kTrajectoryPositionIGain = 0 kTrajectoryPositionDGain = 0 -kTrajectoryAnglePGain = 2.5 +kTrajectoryAnglePGain = 15 kTrajectoryAngleIGain = 0 kTrajectoryAngleDGain = 0 @@ -555,6 +555,10 @@ kJoystickKeyLogPrefix = "DriverStation" kFieldSimTargetKey = "SimTargets" kFieldRelativeTargets = "RelTargets" +kAutonomousPathKey = "auto/path" +kAutonomousPathSample = "auto/desired" +kAutonomousPathError = "auto/error" +kAutonomousChassisSpeeds = "auto/speeds" # photonvision parameters kPhotonvisionCameraName = "cam" diff --git a/deploy/pathplanner/1 Cable Run.path b/deploy/pathplanner/1 Cable Run.path new file mode 100644 index 0000000..f487f1d --- /dev/null +++ b/deploy/pathplanner/1 Cable Run.path @@ -0,0 +1,55 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.8477924693933396, + "y": 0.47588672584126895 + }, + "prevControl": null, + "nextControl": { + "x": 2.7125232028575335, + "y": 0.9139960712258834 + }, + "holonomicAngle": -179.58362856922483, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "top", + "outtake", + "store" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 2.0 + } + }, + { + "anchorPoint": { + "x": 6.167629561692987, + "y": 0.699923225771774 + }, + "prevControl": { + "x": 3.9981186016684345, + "y": 0.7565213489030153 + }, + "nextControl": null, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "intake" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [] +} \ No newline at end of file diff --git a/deploy/pathplanner/1 Loading Station.path b/deploy/pathplanner/1 Loading Station.path new file mode 100644 index 0000000..a74153c --- /dev/null +++ b/deploy/pathplanner/1 Loading Station.path @@ -0,0 +1,53 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.838030226795609, + "y": 4.977925657941868 + }, + "prevControl": null, + "nextControl": { + "x": 2.764534842286831, + "y": 4.616893895774963 + }, + "holonomicAngle": 179.54846311069912, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "top", + "outtake", + "store" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 2.0 + } + }, + { + "anchorPoint": { + "x": 6.361037603805021, + "y": 4.614244789332317 + }, + "prevControl": { + "x": 5.361037603805021, + "y": 4.614244789332317 + }, + "nextControl": null, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [] +} \ No newline at end of file diff --git a/deploy/pathplanner/1 Middle Engaged.path b/deploy/pathplanner/1 Middle Engaged.path new file mode 100644 index 0000000..0a75e35 --- /dev/null +++ b/deploy/pathplanner/1 Middle Engaged.path @@ -0,0 +1,55 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.845175361674196, + "y": 3.2476676859912454 + }, + "prevControl": null, + "nextControl": { + "x": 2.8451753616741953, + "y": 3.2476676859912454 + }, + "holonomicAngle": 179.6309191343329, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "top", + "outtake", + "store" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 2.0 + } + }, + { + "anchorPoint": { + "x": 3.937764299374947, + "y": 3.3123233924500233 + }, + "prevControl": { + "x": 2.937764299374947, + "y": 3.3123233924500233 + }, + "nextControl": null, + "holonomicAngle": 179.47753277640686, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "engage" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [] +} \ No newline at end of file diff --git a/deploy/pathplanner/2 Cable Run Engage.path b/deploy/pathplanner/2 Cable Run Engage.path new file mode 100644 index 0000000..5b04d47 --- /dev/null +++ b/deploy/pathplanner/2 Cable Run Engage.path @@ -0,0 +1,141 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.8477924693933396, + "y": 0.47588672584126895 + }, + "prevControl": null, + "nextControl": { + "x": 3.00703295531803, + "y": 0.6190328580516534 + }, + "holonomicAngle": -179.58362856922483, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "top", + "outtake", + "store" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 2.0 + } + }, + { + "anchorPoint": { + "x": 4.232781770631552, + "y": 0.8404963569750575 + }, + "prevControl": { + "x": 3.6689704223097497, + "y": 0.8639845528719238 + }, + "nextControl": { + "x": 4.796593118953355, + "y": 0.8170081610781913 + }, + "holonomicAngle": -1.0515810979131606, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.474603428397821, + "y": 0.9001205381707219 + }, + "prevControl": { + "x": 5.204214577592418, + "y": 0.7112237986613774 + }, + "nextControl": { + "x": 5.204214577592418, + "y": 0.7112237986613774 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "before", + "waitTime": 2.0 + } + }, + { + "anchorPoint": { + "x": 1.8602250123934378, + "y": 1.0811180623780534 + }, + "prevControl": { + "x": 2.713806737652201, + "y": 0.682511712925323 + }, + "nextControl": { + "x": 2.713806737652201, + "y": 0.682511712925323 + }, + "holonomicAngle": -179.85840042833925, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "top", + "outtake", + "store" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 2.0 + } + }, + { + "anchorPoint": { + "x": 3.848852729366022, + "y": 2.509266250641362 + }, + "prevControl": { + "x": 1.4936854718374593, + "y": 2.4770399966947254 + }, + "nextControl": null, + "holonomicAngle": -179.69148371480446, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "engage" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 1.5723721590909092, + "names": [ + "intake" + ] + } + ] +} \ No newline at end of file diff --git a/deploy/pathplanner/2 Cable Run.path b/deploy/pathplanner/2 Cable Run.path new file mode 100644 index 0000000..7580de2 --- /dev/null +++ b/deploy/pathplanner/2 Cable Run.path @@ -0,0 +1,139 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.8477924693933396, + "y": 0.47588672584126895 + }, + "prevControl": null, + "nextControl": { + "x": 3.1112316414828514, + "y": 0.7067126862955126 + }, + "holonomicAngle": -179.58362856922483, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "top", + "outtake", + "store" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 2.0 + } + }, + { + "anchorPoint": { + "x": 4.147494097703554, + "y": 0.8009716619111923 + }, + "prevControl": { + "x": 3.444088431923326, + "y": 0.7222072637798279 + }, + "nextControl": { + "x": 4.635276694001589, + "y": 0.8555914986362106 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.761313121284342, + "y": 0.935822026057731 + }, + "prevControl": { + "x": 4.637765349334157, + "y": 0.8649062056920268 + }, + "nextControl": { + "x": 4.637765349334157, + "y": 0.8649062056920268 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "before", + "waitTime": 2.0 + } + }, + { + "anchorPoint": { + "x": 1.8602250123934378, + "y": 1.0811180623780534 + }, + "prevControl": { + "x": 3.322000950082423, + "y": 0.4857894303681378 + }, + "nextControl": { + "x": 3.322000950082423, + "y": 0.4857894303681378 + }, + "holonomicAngle": -179.85840042833925, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "top", + "outtake", + "store" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 7.637224791105331, + "y": 0.7429740854347241 + }, + "prevControl": { + "x": 5.198806628009757, + "y": 0.7018117703883869 + }, + "nextControl": null, + "holonomicAngle": 178.22044395463797, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 1.7340820312500003, + "names": [ + "intake" + ] + } + ] +} \ No newline at end of file diff --git a/deploy/pathplanner/2 Loading Station Engage.path b/deploy/pathplanner/2 Loading Station Engage.path new file mode 100644 index 0000000..16f29ac --- /dev/null +++ b/deploy/pathplanner/2 Loading Station Engage.path @@ -0,0 +1,142 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.838030226795609, + "y": 4.977925657941868 + }, + "prevControl": null, + "nextControl": { + "x": 2.962655996906747, + "y": 4.801610449842978 + }, + "holonomicAngle": 179.54846311069912, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "top", + "outtake", + "store" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 2.0 + } + }, + { + "anchorPoint": { + "x": 4.146689109607854, + "y": 4.810104204852603 + }, + "prevControl": { + "x": 3.1929524567943313, + "y": 4.77274141141345 + }, + "nextControl": { + "x": 5.100425762421376, + "y": 4.847466998291757 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.4884080378753515, + "y": 4.6094291892291945 + }, + "prevControl": { + "x": 5.112661561074175, + "y": 4.8389287592937595 + }, + "nextControl": { + "x": 5.112661561074175, + "y": 4.8389287592937595 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "store" + ], + "executionBehavior": "parallel", + "waitBehavior": "before", + "waitTime": 2.0 + } + }, + { + "anchorPoint": { + "x": 1.8378109454999225, + "y": 4.4151312778540595 + }, + "prevControl": { + "x": 3.059739348372424, + "y": 4.711946954009603 + }, + "nextControl": { + "x": 3.059739348372424, + "y": 4.711946954009603 + }, + "holonomicAngle": 179.6485610195212, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "top", + "outtake" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 2.0 + } + }, + { + "anchorPoint": { + "x": 3.8280034139040136, + "y": 3.2760814442800683 + }, + "prevControl": { + "x": 0.8021291381911144, + "y": 2.6242751041911525 + }, + "nextControl": null, + "holonomicAngle": 179.43733934488517, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "engage" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 1.4635475852272728, + "names": [ + "intake" + ] + } + ] +} \ No newline at end of file diff --git a/deploy/pathplanner/2 Loading Station.path b/deploy/pathplanner/2 Loading Station.path new file mode 100644 index 0000000..d4d6d0e --- /dev/null +++ b/deploy/pathplanner/2 Loading Station.path @@ -0,0 +1,151 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.838030226795609, + "y": 4.977925657941868 + }, + "prevControl": null, + "nextControl": { + "x": 2.5248824744589657, + "y": 4.7507467725446615 + }, + "holonomicAngle": 179.54846311069912, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "top", + "outtake", + "store" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 2.0 + } + }, + { + "anchorPoint": { + "x": 3.5171297758594284, + "y": 4.644310179845025 + }, + "prevControl": { + "x": 3.2673144018723597, + "y": 4.662223782132235 + }, + "nextControl": { + "x": 4.165852754706652, + "y": 4.59779196420372 + }, + "holonomicAngle": 1.2709368398322358, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.719965594220838, + "y": 4.609191688863736 + }, + "prevControl": { + "x": 5.216613526702508, + "y": 4.938842232726347 + }, + "nextControl": { + "x": 5.216613526702508, + "y": 4.938842232726347 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "store" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.3985337552258414, + "y": 4.5592924342975305 + }, + "prevControl": { + "x": 4.573867374678349, + "y": 4.631310666738431 + }, + "nextControl": { + "x": 2.5910431449841402, + "y": 4.509813677372905 + }, + "holonomicAngle": -178.06772976953638, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 1.8378109454999225, + "y": 4.4151312778540595 + }, + "prevControl": { + "x": 2.318922651124626, + "y": 4.54468843657916 + }, + "nextControl": null, + "holonomicAngle": 179.6485610195212, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "outtake" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 1.5219637784090907, + "names": [ + "intake" + ] + }, + { + "position": 2.1897194602272734, + "names": [ + "store" + ] + }, + { + "position": 2.9529119318181802, + "names": [ + "top" + ] + } + ] +} \ No newline at end of file diff --git a/deploy/pathplanner/2 Middle Engaged.path b/deploy/pathplanner/2 Middle Engaged.path new file mode 100644 index 0000000..aec0539 --- /dev/null +++ b/deploy/pathplanner/2 Middle Engaged.path @@ -0,0 +1,175 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.845175361674196, + "y": 3.2476676859912454 + }, + "prevControl": null, + "nextControl": { + "x": 2.8451753616741953, + "y": 3.2476676859912454 + }, + "holonomicAngle": 179.6309191343329, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "top", + "outtake", + "store" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 2.0 + } + }, + { + "anchorPoint": { + "x": 3.9128791910382343, + "y": 3.1954943359269468 + }, + "prevControl": { + "x": 2.9128791910382343, + "y": 3.1954943359269468 + }, + "nextControl": { + "x": 4.912879191038235, + "y": 3.1954943359269468 + }, + "holonomicAngle": 2.4418272627611612, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "engage" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.5976009621896905, + "y": 3.375397856092817 + }, + "prevControl": { + "x": 5.762989756180399, + "y": 3.3354440995270442 + }, + "nextControl": { + "x": 5.762989756180399, + "y": 3.3354440995270442 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "store" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.9600817623322992, + "y": 2.488790035783717 + }, + "prevControl": { + "x": 5.6778134285953845, + "y": 2.7697839651238043 + }, + "nextControl": { + "x": 3.017920308165017, + "y": 2.3346672153928125 + }, + "holonomicAngle": 177.61719631358952, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 1.862580555481101, + "y": 2.7000224918522626 + }, + "prevControl": { + "x": 2.517550451368795, + "y": 2.592181147609218 + }, + "nextControl": { + "x": 2.517550451368795, + "y": 2.592181147609218 + }, + "holonomicAngle": 179.7667362175226, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "outtake", + "store" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.9854778628706726, + "y": 2.637775697683865 + }, + "prevControl": { + "x": 2.8353754173907317, + "y": 2.664836686562106 + }, + "nextControl": null, + "holonomicAngle": -177.77411237870479, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "engage" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 1.4096457741477277, + "names": [ + "intake" + ] + }, + { + "position": 3.29141512784091, + "names": [ + "top" + ] + } + ] +} \ No newline at end of file diff --git a/deploy/pathplanner/Test Path.path b/deploy/pathplanner/Test Path.path new file mode 100644 index 0000000..3038ce7 --- /dev/null +++ b/deploy/pathplanner/Test Path.path @@ -0,0 +1,106 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.8494931430787334, + "y": 0.5177987479222964 + }, + "prevControl": null, + "nextControl": { + "x": 2.872291682271884, + "y": 1.1030395045264567 + }, + "holonomicAngle": 179.9190133455032, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "place" + ], + "executionBehavior": "sequential", + "waitBehavior": "after", + "waitTime": 2.0 + } + }, + { + "anchorPoint": { + "x": 6.472051149042473, + "y": 0.9163849006876117 + }, + "prevControl": { + "x": 3.2754380011257704, + "y": 0.5513523564802724 + }, + "nextControl": { + "x": 3.2754380011257704, + "y": 0.5513523564802724 + }, + "holonomicAngle": 1.5263364437454054, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 1.8492865611718712, + "y": 1.0805843394344221 + }, + "prevControl": { + "x": 2.47893994979319, + "y": 0.9472391991949426 + }, + "nextControl": { + "x": 2.47893994979319, + "y": 0.9472391991949426 + }, + "holonomicAngle": 179.181914952578, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "wait" + ], + "executionBehavior": "parallel", + "waitBehavior": "before", + "waitTime": 2.0 + } + }, + { + "anchorPoint": { + "x": 3.918977476590525, + "y": 2.4007222901542122 + }, + "prevControl": { + "x": 2.8461450620786466, + "y": 2.245225734504966 + }, + "nextControl": null, + "holonomicAngle": -176.86788159538688, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "maxVelocity": 4.5, + "maxAcceleration": 4.5, + "isReversed": null, + "markers": [] +} \ No newline at end of file diff --git a/networktables.json b/networktables.json index f8524da..3ace07f 100644 --- a/networktables.json +++ b/networktables.json @@ -6,5 +6,21 @@ "properties": { "persistent": true } + }, + { + "name": "/Preferences/arm/smooth", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/arm/useEndstops", + "type": "boolean", + "value": true, + "properties": { + "persistent": true + } } ] diff --git a/requirements.txt b/requirements.txt index ceca0da..b989cb5 100644 --- a/requirements.txt +++ b/requirements.txt @@ -17,5 +17,6 @@ robotpy-installer==2023.0.1 robotpy-navx==2023.0.3 robotpy-wpimath==2023.3.2.1 robotpy-wpiutil==2023.3.2.1 +robotpy-pathplannerlib==2023.3.4.1 wpilib==2023.3.2.1 robotpy-photonvision==2023.3.0 diff --git a/robotcontainer.py b/robotcontainer.py index d0e2c50..37b1928 100644 --- a/robotcontainer.py +++ b/robotcontainer.py @@ -1,3 +1,4 @@ +import os import wpilib from wpimath.geometry import Pose2d import commands2 @@ -14,6 +15,7 @@ from commands.drive.robotrelativedrive import RobotRelativeDrive from commands.drive.absoluterelativedrive import AbsoluteRelativeDrive from commands.defensestate import DefenseState +from commands.auto.autonomousaction import AutonomousRoutine from subsystems.drivesubsystem import DriveSubsystem from subsystems.loggingsubsystem import LoggingSubsystem @@ -62,8 +64,13 @@ def __init__(self) -> None: self.chooser = wpilib.SendableChooser() # Add commands to the autonomous command chooser - self.chooser.addOption("Complex Auto", self.complexAuto) - self.chooser.addOption("Target Auto", self.driveToTarget) + pathsPath = os.path.join(wpilib.getDeployDirectory(), "pathplanner") + for file in os.listdir(pathsPath): + relevantName = file.split(".")[0] + self.chooser.addOption( + relevantName, AutonomousRoutine(self.drive, relevantName, []) + ) + self.chooser.setDefaultOption("Simple Auto", self.simpleAuto) # Put the chooser on the dashboard