diff --git a/src/main/deploy/pathplanner/paths/L NY auto.path b/src/main/deploy/pathplanner/paths/L NY auto.path new file mode 100644 index 0000000000..f46a9d1774 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/L NY auto.path @@ -0,0 +1,258 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5522953056768554, + "y": 5.41084760507642 + }, + "prevControl": null, + "nextControl": { + "x": 4.552295305676853, + "y": 5.41084760507642 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.6940319095705965, + "y": 5.41084760507642 + }, + "prevControl": { + "x": 4.6940319095705965, + "y": 5.41084760507642 + }, + "nextControl": { + "x": 6.6940319095705965, + "y": 5.41084760507642 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.6794890602256185, + "y": 7.3022578352438146 + }, + "prevControl": { + "x": 6.763881454226824, + "y": 7.847734468456682 + }, + "nextControl": { + "x": 7.894263593013083, + "y": 7.174305109341134 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.802109090702329, + "y": 4.398072973526201 + }, + "prevControl": { + "x": 7.808890357136904, + "y": 5.405091039060494 + }, + "nextControl": { + "x": 7.7964243881914115, + "y": 3.553894650655022 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.802109090702329, + "y": 2.0691899790756905 + }, + "prevControl": { + "x": 7.85209956673035, + "y": 2.705702129958151 + }, + "nextControl": { + "x": 7.752118614674308, + "y": 1.43267782819323 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.024043850072781, + "y": 3.0240205376637563 + }, + "prevControl": { + "x": 6.100431545487207, + "y": 2.3357022651852404 + }, + "nextControl": { + "x": 5.959716953238719, + "y": 3.6036607305312964 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.024043850072781, + "y": 4.956919122998545 + }, + "prevControl": { + "x": 5.982457402927915, + "y": 4.683371061904318 + }, + "nextControl": { + "x": 6.135543804585153, + "y": 5.690345478529841 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.386491482441776, + "y": 5.41084760507642 + }, + "prevControl": { + "x": 5.14376426835919, + "y": 5.127776229818807 + }, + "nextControl": { + "x": 1.8541649608806403, + "y": 5.657683371997816 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.5946044282205241, + "y": 5.869413607623726 + }, + "prevControl": { + "x": 0.6202385938170157, + "y": 6.118095914099625 + }, + "nextControl": { + "x": 0.5539613468886462, + "y": 5.475126726318636 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.386491482441776, + "y": 5.41084760507642 + }, + "prevControl": { + "x": 2.0135040453320805, + "y": 5.439190048362276 + }, + "nextControl": { + "x": 4.695768229166666, + "y": 5.383820335243814 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.535725925673216, + "y": 5.227441150382097 + }, + "prevControl": { + "x": 6.810976221342795, + "y": 5.282393274654294 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.0468064692982455, + "rotationDegrees": 135.0 + }, + { + "waypointRelativePos": 1.667985882675439, + "rotationDegrees": 111.7563825865353 + }, + { + "waypointRelativePos": 2.3293328536184212, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 3.8280393366228065, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 4.957648026315792, + "rotationDegrees": -55.0 + }, + { + "waypointRelativePos": 7.0, + "rotationDegrees": -55.0 + }, + { + "waypointRelativePos": 8.0, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 9.007846765350875, + "rotationDegrees": 35.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 2.4030172413793105, + "maxWaypointRelativePos": 4.332083057029168, + "constraints": { + "maxVelocity": 1.6, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 7.253191312997349, + "maxWaypointRelativePos": 8.969040119363441, + "constraints": { + "maxVelocity": 0.7, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.2, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 35.798885165620696 + }, + "reversed": false, + "folder": "Steal Auto", + "idealStartingState": { + "velocity": 0, + "rotation": 135.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/R NY auto.path b/src/main/deploy/pathplanner/paths/R NY auto.path new file mode 100644 index 0000000000..e71bca7991 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/R NY auto.path @@ -0,0 +1,258 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.386491482441776, + "y": 2.546821903935186 + }, + "prevControl": null, + "nextControl": { + "x": 7.871491482441776, + "y": 2.546821903935186 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.488554216867469, + "y": 0.7457590361445796 + }, + "prevControl": { + "x": 4.696638554216866, + "y": 0.7129759036144571 + }, + "nextControl": { + "x": 8.738544874235794, + "y": 0.7479203386578506 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.597831325301206, + "y": 4.198915662650602 + }, + "prevControl": { + "x": 8.586903614457835, + "y": 3.6743855421686744 + }, + "nextControl": { + "x": 8.608497340976083, + "y": 4.710884415044804 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.488554216867469, + "y": 7.00733734939759 + }, + "prevControl": { + "x": 8.654103665985396, + "y": 6.820005078027302 + }, + "nextControl": { + "x": 8.073301204819277, + "y": 7.477228915662651 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.072965711805556, + "y": 5.235279079861112 + }, + "prevControl": { + "x": 6.212869513468274, + "y": 5.9135445518695695 + }, + "nextControl": { + "x": 6.0079638554216865, + "y": 4.920144578313253 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.9670703125, + "y": 3.037851128472222 + }, + "prevControl": { + "x": 6.046472190130049, + "y": 3.302904549773223 + }, + "nextControl": { + "x": 5.852973596643519, + "y": 2.656981987847222 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.2834683159722227, + "y": 2.8499425636574074 + }, + "prevControl": { + "x": 4.908961573483471, + "y": 2.4156791317948003 + }, + "nextControl": { + "x": 2.6869373553240745, + "y": 3.009310546875 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.1997791521990746, + "y": 4.4900746527777775 + }, + "prevControl": { + "x": 2.1725217035459012, + "y": 3.9817779010953296 + }, + "nextControl": { + "x": 2.2946519820601856, + "y": 6.259262659143519 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.6419226707175931, + "y": 5.869413607623726 + }, + "prevControl": { + "x": 1.1862932581018524, + "y": 5.5607073206018525 + }, + "nextControl": { + "x": 0.2821988381236935, + "y": 6.073408847790796 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.155462167245371, + "y": 5.7135798611111115 + }, + "prevControl": { + "x": 1.7824747301356756, + "y": 5.741922304396968 + }, + "nextControl": { + "x": 4.464738913970253, + "y": 5.686552591278506 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.454067418981482, + "y": 5.235279079861112 + }, + "prevControl": { + "x": 6.729317714651061, + "y": 5.290231204133309 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": -135.0 + }, + { + "waypointRelativePos": 1.1350740131578956, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.8280393366228065, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 3.957648026315792, + "rotationDegrees": 50.48239708370023 + }, + { + "waypointRelativePos": 5.0, + "rotationDegrees": 50.0 + }, + { + "waypointRelativePos": 6.0, + "rotationDegrees": 50.0 + }, + { + "waypointRelativePos": 8.0, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 9.007846765350875, + "rotationDegrees": 35.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.2697778239202657, + "maxWaypointRelativePos": 2.8085288621262414, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 6.3429453903654505, + "maxWaypointRelativePos": 8.969040119363441, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.2, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 35.798885165620696 + }, + "reversed": false, + "folder": "Steal Auto", + "idealStartingState": { + "velocity": 0, + "rotation": -135.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/autonomous/AutonomousConstants.java b/src/main/java/frc/robot/autonomous/AutonomousConstants.java index 87d45ff620..a14039df62 100644 --- a/src/main/java/frc/robot/autonomous/AutonomousConstants.java +++ b/src/main/java/frc/robot/autonomous/AutonomousConstants.java @@ -35,6 +35,9 @@ public class AutonomousConstants { public static final double STEAL_START_SECOND_SINCE_AUTO_BEGAN = 3.5; public static final double SIDE_STEAL_START_SECOND_SINCE_AUTO_BEGAN = 3.5; + public static final double NY_AUTO_TIME_TO_START_PASSING = 3.0; + public static final double NY_AUTO_TIME_TO_PASS = 9; + public static final Double DEFAULT_STUCK_DEBOUNCE_SECONDS = 2.0; } diff --git a/src/main/java/frc/robot/autonomous/AutosBuilder.java b/src/main/java/frc/robot/autonomous/AutosBuilder.java index 918825d6c2..4cdf5f99ad 100644 --- a/src/main/java/frc/robot/autonomous/AutosBuilder.java +++ b/src/main/java/frc/robot/autonomous/AutosBuilder.java @@ -430,6 +430,30 @@ public static List> getAutoList( stuckDebounceSeconds, AllianceSide.OUTPOST, true + ), + getNewYorkAuto( + robot, + resetSubsystems, + openIntake, + scoreSequence, + passSequence, + pathfindingConstraints, + regularIsNearEndOfPathTolerance, + stuckIsNearEndOfPathTolerance, + stuckDebounceSeconds, + AllianceSide.OUTPOST + ), + getNewYorkAuto( + robot, + resetSubsystems, + openIntake, + scoreSequence, + passSequence, + pathfindingConstraints, + regularIsNearEndOfPathTolerance, + stuckIsNearEndOfPathTolerance, + stuckDebounceSeconds, + AllianceSide.DEPOT ) ); } @@ -1131,6 +1155,54 @@ private static Supplier getMiddleAuto( ); } + private static Supplier getNewYorkAuto( + Robot robot, + Supplier resetSubsystems, + Supplier openIntake, + Supplier scoreSequence, + Supplier passSequence, + PathConstraints pathfindingConstraints, + Pose2d regularIsNearEndOfPathTolerance, + Pose2d stuckIsNearEndOfPathTolerance, + double stuckDebounceSeconds, + AllianceSide startingSide + ) { + return () -> new PathPlannerAutoWrapper( + new ParallelCommandGroup( + PathFollowingCommandsBuilder + .followAdjustedPathThenStop( + robot.getSwerve(), + () -> robot.getPoseEstimator().getEstimatedPose(), + startingSide == AllianceSide.OUTPOST + ? PathHelper.PATH_PLANNER_PATHS.get("R NY auto") + : PathHelper.PATH_PLANNER_PATHS.get("L NY auto"), + pathfindingConstraints, + regularIsNearEndOfPathTolerance, + stuckIsNearEndOfPathTolerance, + stuckDebounceSeconds, + robot.getSwerve().getLogPath() + ) + .asProxy() + .andThen(new InstantCommand(() -> hasPathEnded = true)), + new SequentialCommandGroup( + resetSubsystems.get(), + new ParallelCommandGroup( + new WaitCommand(AutonomousConstants.NY_AUTO_TIME_TO_START_PASSING).andThen(passSequence.get()) + .withTimeout(AutonomousConstants.NY_AUTO_TIME_TO_PASS) + .andThen(scoreSequence.get()), + openIntake.get() + ) + ) + ), + new Pose2d(), + startingSide.name() + " NY auto", + startingSide == AllianceSide.OUTPOST + ? PathHelper.PATH_PLANNER_PATHS.get("R NY auto") + : PathHelper.PATH_PLANNER_PATHS.get("L NY auto") + ); + } + + private static Command getAllianceSideToStartingLineAuto( Robot robot, AllianceSide allianceSide,