diff --git a/src/main/deploy/pathplanner/paths/L quarter.path b/src/main/deploy/pathplanner/paths/L quarter.path index c3adc3820e..d9774957d4 100644 --- a/src/main/deploy/pathplanner/paths/L quarter.path +++ b/src/main/deploy/pathplanner/paths/L quarter.path @@ -105,7 +105,7 @@ }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "DepotBalls" } ], "rotationTargets": [ diff --git a/src/main/deploy/pathplanner/paths/L seven.path b/src/main/deploy/pathplanner/paths/L seven.path new file mode 100644 index 0000000000..1a6dab3949 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/L seven.path @@ -0,0 +1,139 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.395, + "y": 5.71 + }, + "prevControl": null, + "nextControl": { + "x": 4.4152499999999995, + "y": 5.601636363636365 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.105568181818182, + "y": 5.477954545454545 + }, + "prevControl": { + "x": 5.889893689293765, + "y": 5.351524670526438 + }, + "nextControl": { + "x": 6.404465909090909, + "y": 5.653170454545455 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.239, + "y": 7.4465568181818185 + }, + "prevControl": { + "x": 8.12296609654913, + "y": 8.049933116126335 + }, + "nextControl": { + "x": 8.290534090909093, + "y": 7.178579545454545 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.239, + "y": 4.282363636363637 + }, + "prevControl": { + "x": 8.249386363636363, + "y": 5.416113636363637 + }, + "nextControl": { + "x": 8.236709828484035, + "y": 4.032374126354862 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.858204545454544, + "y": 5.251204545454545 + }, + "prevControl": { + "x": 7.393920454545453, + "y": 4.746170454545453 + }, + "nextControl": { + "x": 5.620716873612638, + "y": 5.3293045180736955 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.698, + "y": 5.973061658398299 + }, + "prevControl": { + "x": 3.374261363636363, + "y": 5.405806818181818 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "DepotBalls" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.7940251572327044, + "rotationDegrees": 135.0 + }, + { + "waypointRelativePos": 1.85, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 3.042452830188687, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 3.977987421383667, + "rotationDegrees": 45.0 + }, + { + "waypointRelativePos": 4.638364779874225, + "rotationDegrees": 45.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.2, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 135.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/R quarter.path b/src/main/deploy/pathplanner/paths/R quarter.path index c104be027c..ebe4302f9c 100644 --- a/src/main/deploy/pathplanner/paths/R quarter.path +++ b/src/main/deploy/pathplanner/paths/R quarter.path @@ -12,7 +12,7 @@ "y": 2.5147127350855207 }, "isLocked": false, - "linkedName": null + "linkedName": "starting line right" }, { "anchor": { diff --git a/src/main/deploy/pathplanner/paths/R seven.path b/src/main/deploy/pathplanner/paths/R seven.path new file mode 100644 index 0000000000..a101b63e38 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/R seven.path @@ -0,0 +1,169 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.395203052662038, + "y": 2.5 + }, + "prevControl": null, + "nextControl": { + "x": 3.700185248731214, + "y": 2.5 + }, + "isLocked": false, + "linkedName": "starting line right" + }, + { + "anchor": { + "x": 5.682, + "y": 2.3335663338034864 + }, + "prevControl": { + "x": 4.931004907178515, + "y": 2.4293133162224 + }, + "nextControl": { + "x": 6.432995092821486, + "y": 2.237819351384573 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.239079545454544, + "y": 1.5407500000000005 + }, + "prevControl": { + "x": 8.012329545454545, + "y": -0.32478409090909044 + }, + "nextControl": { + "x": 8.317278169587606, + "y": 2.1841114076401977 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.239079545454544, + "y": 3.901011363636364 + }, + "prevControl": { + "x": 8.302716577054621, + "y": 2.8510003420115657 + }, + "nextControl": { + "x": 8.213202227343402, + "y": 4.327987112561106 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.9281027104195285, + "y": 2.644094151207196 + }, + "prevControl": { + "x": 6.173522772446481, + "y": 2.6977797897716598 + }, + "nextControl": { + "x": 5.514065620542081, + "y": 2.5535235378031382 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.395203052662038, + "y": 2.1653637660485026 + }, + "prevControl": { + "x": 3.6315871896001, + "y": 2.246742754762098 + }, + "nextControl": { + "x": 3.09453637660485, + "y": 2.0618544935805994 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.4809272467902993, + "y": 0.71623395149786 + }, + "prevControl": { + "x": 0.730063686351519, + "y": 0.7369953214612982 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.9722814498933869, + "rotationDegrees": 135.0 + }, + { + "waypointRelativePos": 2.046908315565027, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.910447761194019, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 3.955223880597019, + "rotationDegrees": 45.0 + }, + { + "waypointRelativePos": 5.15, + "rotationDegrees": 45.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 5.6, + "maxWaypointRelativePos": 6.0, + "constraints": { + "maxVelocity": 1.5, + "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, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 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..b37309417f 100644 --- a/src/main/java/frc/robot/autonomous/AutosBuilder.java +++ b/src/main/java/frc/robot/autonomous/AutosBuilder.java @@ -82,6 +82,30 @@ public static List> getAutoList( stuckIsNearEndOfPathTolerance, stuckDebounceSeconds, AllianceSide.DEPOT + ), + getSevenAuto( + robot, + resetSubsystems, + openIntake, + closeIntake, + scoreSequence, + pathfindingConstraints, + regularIsNearEndOfPathTolerance, + stuckIsNearEndOfPathTolerance, + stuckDebounceSeconds, + AllianceSide.DEPOT + ), + getSevenAuto( + robot, + resetSubsystems, + openIntake, + closeIntake, + scoreSequence, + pathfindingConstraints, + regularIsNearEndOfPathTolerance, + stuckIsNearEndOfPathTolerance, + stuckDebounceSeconds, + AllianceSide.OUTPOST ) ); } @@ -222,6 +246,63 @@ private static Supplier getQuarterAuto( ); } + private static Supplier getSevenAuto( + 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 seven") + : PathHelper.PATH_PLANNER_PATHS.get("R seven"), + 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 seven" : "L seven" + ); + } + private static Command getAllianceSideToStartingLineAuto( Robot robot, AllianceSide allianceSide,