diff --git a/src/main/deploy/pathplanner/autos/B-L2.auto b/src/main/deploy/pathplanner/autos/B-L2.auto new file mode 100644 index 0000000..6d96e83 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/B-L2.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "B_R4" + } + }, + { + "type": "named", + "data": { + "name": "L2" + } + }, + { + "type": "named", + "data": { + "name": "AlgaeL3" + } + }, + { + "type": "path", + "data": { + "pathName": "R4_P" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/R4_P.path b/src/main/deploy/pathplanner/paths/R4_P.path new file mode 100644 index 0000000..92dca2c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/R4_P.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.1, + "y": 4.05 + }, + "prevControl": null, + "nextControl": { + "x": 7.228887668477433, + "y": 0.6385793840777492 + }, + "isLocked": false, + "linkedName": "R4" + }, + { + "anchor": { + "x": 7.2735, + "y": 0.905 + }, + "prevControl": { + "x": 7.265791075075885, + "y": 1.1548811166865443 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -90.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0.0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c6cf064..231af7e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -173,6 +173,18 @@ public RobotContainer() { NamedCommands.registerCommand("Intake", multiSubsystemCommands.loadGamepiece()); + // AutoAlign + Algae Removal + NamedCommands.registerCommand("AlgaeL2", + multiSubsystemCommands.setOverallSetpoint(OverallPosition.L2) + .andThen(multiSubsystemCommands.waitForOverallMechanism()) + .andThen(multiSubsystemCommands.loadAlgae())); + + // AutoAlign + Algae Removal + NamedCommands.registerCommand("AlgaeL3", + multiSubsystemCommands.setOverallSetpoint(OverallPosition.L3) + .andThen(multiSubsystemCommands.waitForOverallMechanism()) + .andThen(multiSubsystemCommands.loadAlgae())); + configureBindings(); }