diff --git a/src/main/deploy/pathplanner/paths/L half.path b/src/main/deploy/pathplanner/paths/L half.path new file mode 100644 index 0000000000..430b26f138 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/L half.path @@ -0,0 +1,193 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.34, + "y": 5.71 + }, + "prevControl": null, + "nextControl": { + "x": 4.507589938528869, + "y": 5.71 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.0, + "y": 5.71 + }, + "prevControl": { + "x": 5.75, + "y": 5.71 + }, + "nextControl": { + "x": 7.0, + "y": 5.71 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.1, + "y": 6.370427960057061 + }, + "prevControl": { + "x": 7.85, + "y": 6.370427960057061 + }, + "nextControl": { + "x": 8.95, + "y": 6.370427960057061 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.1, + "y": 4.621 + }, + "prevControl": { + "x": 8.95, + "y": 4.621 + }, + "nextControl": { + "x": 7.1, + "y": 4.621 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.0, + "y": 5.71 + }, + "prevControl": { + "x": 7.999999999999999, + "y": 5.71 + }, + "nextControl": { + "x": 5.75, + "y": 5.71 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.34, + "y": 5.71 + }, + "prevControl": { + "x": 4.339999999999998, + "y": 5.71 + }, + "nextControl": { + "x": 2.34, + "y": 5.71 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.7, + "y": 6.0 + }, + "prevControl": { + "x": 1.7, + "y": 6.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.7261410788381721, + "rotationDegrees": -135.0 + }, + { + "waypointRelativePos": 1.2157676348547723, + "rotationDegrees": -135.0 + }, + { + "waypointRelativePos": 4.360995850622402, + "rotationDegrees": -45.0 + }, + { + "waypointRelativePos": 5.0, + "rotationDegrees": -45.0 + }, + { + "waypointRelativePos": 5.782515991471213, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 4.7643484132343055, + "maxWaypointRelativePos": 5.8, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 5.801485482781905, + "maxWaypointRelativePos": 6.0, + "constraints": { + "maxVelocity": 0.5, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 8.1, + "y": 5.5 + }, + "rotationOffset": -90.0, + "minWaypointRelativePos": 1.5232950708980328, + "maxWaypointRelativePos": 3.61377447670493, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.2, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0.0, + "rotation": -135.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/R half.path b/src/main/deploy/pathplanner/paths/R half.path new file mode 100644 index 0000000000..3abadb0b79 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/R half.path @@ -0,0 +1,188 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.34, + "y": 2.5 + }, + "prevControl": null, + "nextControl": { + "x": 4.507589938528869, + "y": 2.5 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.0, + "y": 2.5 + }, + "prevControl": { + "x": 5.75, + "y": 2.5 + }, + "nextControl": { + "x": 7.0, + "y": 2.5 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.1, + "y": 1.915024096385541 + }, + "prevControl": { + "x": 7.85, + "y": 1.915024096385541 + }, + "nextControl": { + "x": 8.950000000000001, + "y": 1.915024096385541 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.1, + "y": 3.627432239657632 + }, + "prevControl": { + "x": 8.95, + "y": 3.627432239657632 + }, + "nextControl": { + "x": 7.1, + "y": 3.627432239657632 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.0, + "y": 2.5 + }, + "prevControl": { + "x": 7.999999999999999, + "y": 2.5 + }, + "nextControl": { + "x": 5.75, + "y": 2.5 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.34, + "y": 2.5 + }, + "prevControl": { + "x": 4.339999999999998, + "y": 2.5 + }, + "nextControl": { + "x": 1.3399999999999999, + "y": 2.5000000000000004 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.6, + "y": 0.67 + }, + "prevControl": { + "x": 2.6, + "y": 0.67 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.8, + "rotationDegrees": 135.0 + }, + { + "waypointRelativePos": 1.2157676348547723, + "rotationDegrees": 135.0 + }, + { + "waypointRelativePos": 2.0, + "rotationDegrees": -135.0 + }, + { + "waypointRelativePos": 3.0, + "rotationDegrees": -0.0 + }, + { + "waypointRelativePos": 3.6076759061833608, + "rotationDegrees": 45.0 + }, + { + "waypointRelativePos": 4.360995850622402, + "rotationDegrees": 45.0 + }, + { + "waypointRelativePos": 5.0, + "rotationDegrees": 45.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 4.780553679946003, + "maxWaypointRelativePos": 6.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 8.1, + "y": 2.7 + }, + "rotationOffset": 90.0, + "minWaypointRelativePos": 1.4584740040513338, + "maxWaypointRelativePos": 3.61377447670494, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.2, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0.0, + "rotation": 135.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/autonomous/AutosBuilder.java b/src/main/java/frc/robot/autonomous/AutosBuilder.java index 2fb185350e..fcd9f8c0c9 100644 --- a/src/main/java/frc/robot/autonomous/AutosBuilder.java +++ b/src/main/java/frc/robot/autonomous/AutosBuilder.java @@ -35,6 +35,30 @@ public static List> getAutoList( double stuckDebounceSeconds ) { return List.of( + halfAuto( + robot, + resetSubsystems, + openIntake, + closeIntake, + scoreSequence, + pathfindingConstraints, + regularIsNearEndOfPathTolerance, + stuckIsNearEndOfPathTolerance, + stuckDebounceSeconds, + AllianceSide.OUTPOST + ), + halfAuto( + robot, + resetSubsystems, + openIntake, + closeIntake, + scoreSequence, + pathfindingConstraints, + regularIsNearEndOfPathTolerance, + stuckIsNearEndOfPathTolerance, + stuckDebounceSeconds, + AllianceSide.DEPOT + ), getHorseshoeAuto( robot, resetSubsystems, @@ -86,6 +110,64 @@ public static List> getAutoList( ); } + private static Supplier halfAuto( + Robot robot, + Supplier resetSubsystems, + Supplier openIntake, + Supplier closeIntake, + Supplier scoreSequence, + PathConstraints pathfindingConstraints, + Pose2d regularIsNearEndOfPathTolerance, + Pose2d stuckIsNearEndOfPathTolerance, + double stuckDebounceSeconds, + AllianceSide startingSide + ) { + return () -> new PathPlannerAutoWrapper( + new ParallelCommandGroup( + PathFollowingCommandsBuilder + .followAdjustedPathThenStop( + robot.getSwerve(), + () -> robot.getPoseEstimator().getEstimatedPose(), + startingSide == AllianceSide.DEPOT + ? PathHelper.PATH_PLANNER_PATHS.get("L half") + : PathHelper.PATH_PLANNER_PATHS.get("R half"), + pathfindingConstraints, + regularIsNearEndOfPathTolerance, + stuckIsNearEndOfPathTolerance, + stuckDebounceSeconds, + robot.getSwerve().getLogPath() + + ) + .asProxy() + .alongWith(new InstantCommand(() -> hasPathEnded = false)) + .andThen(new InstantCommand(() -> hasPathEnded = true)), + new SequentialCommandGroup( + resetSubsystems.get(), + new ParallelCommandGroup( + new WaitCommand(AutonomousConstants.TIME_TO_WAIT_TO_START_SHOOTING_AFTER_AUTO_START).andThen(scoreSequence.get()), + openIntake.get() + .until(() -> hasPathEnded) + .andThen( + new ParallelCommandGroup( + new WaitCommand(AutonomousConstants.TIME_TO_WAIT_TO_START_WIGGLE_AFTER_PATH_END) + .andThen( + robot.getSwerve() + .getCommandsBuilder() + .wiggle(AutonomousConstants.WIGGLE_RANGE, AutonomousConstants.TIME_BETWEEN_WIGGLES_SECONDS) + ) + .asProxy(), + new WaitCommand(AutonomousConstants.TIME_TO_WAIT_TO_CLOSE_INTAKE_AFTER_PATH_END_SECONDS) + .andThen(closeIntake.get()) + ) + ) + ) + ) + ), + new Pose2d(), + startingSide == AllianceSide.OUTPOST ? "R half" : "L half" + ); + } + private static Supplier getHorseshoeAuto( Robot robot, Supplier resetSubsystems,