diff --git a/src/main/deploy/pathplanner/paths/L double swipe.path b/src/main/deploy/pathplanner/paths/L double swipe.path new file mode 100644 index 0000000000..aeccbd0d3e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/L double swipe.path @@ -0,0 +1,177 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.698, + "y": 5.973061658398299 + }, + "prevControl": null, + "nextControl": { + "x": 1.8654972376756007, + "y": 5.958348923312778 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.569672364672364, + "y": 5.71 + }, + "prevControl": { + "x": 5.319684140380445, + "y": 5.707573538507863 + }, + "nextControl": { + "x": 6.673958188657408, + "y": 5.720718532986112 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.27038794626047, + "y": 4.664813513445185 + }, + "prevControl": { + "x": 8.701588101689149, + "y": 7.251925360442475 + }, + "nextControl": { + "x": 8.201584412594418, + "y": 4.252006526198716 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.281156626506024, + "y": 5.166018072289157 + }, + "prevControl": { + "x": 7.4229657958013675, + "y": 3.494083217249549 + }, + "nextControl": { + "x": 5.975180722891566, + "y": 5.61405421686747 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.569672364672364, + "y": 5.5811697574893016 + }, + "prevControl": { + "x": 6.057980778775294, + "y": 5.5811697574893016 + }, + "nextControl": { + "x": 5.081363950569434, + "y": 5.5811697574893016 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.508573466476462, + "y": 5.5811697574893016 + }, + "prevControl": { + "x": 4.098255113043399, + "y": 5.5811697574893016 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.0, + "rotationDegrees": -135.0 + }, + { + "waypointRelativePos": 1.3571134868421066, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 1.7769325657894743, + "rotationDegrees": 96.86659894451316 + }, + { + "waypointRelativePos": 3.0, + "rotationDegrees": -52.86053644571499 + }, + { + "waypointRelativePos": 3.9, + "rotationDegrees": -45.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 3.8, + "maxWaypointRelativePos": 4.4, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 4.7, + "maxWaypointRelativePos": 5.0, + "constraints": { + "maxVelocity": 0.5, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 7.9, + "y": 5.01 + }, + "rotationOffset": -90.0, + "minWaypointRelativePos": 1.9292722176079757, + "maxWaypointRelativePos": 2.8679401993355524, + "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, + "rotation": -45.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 double swipe.path b/src/main/deploy/pathplanner/paths/R double swipe.path new file mode 100644 index 0000000000..faa4049f29 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/R double swipe.path @@ -0,0 +1,172 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.8610120481968198, + "y": 0.636 + }, + "prevControl": null, + "nextControl": { + "x": 2.2269759036185066, + "y": 2.024301204820622 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.395203052662038, + "y": 2.5 + }, + "prevControl": { + "x": 2.501209268016166, + "y": 2.5 + }, + "nextControl": { + "x": 4.28919683730791, + "y": 2.5 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.569672364672364, + "y": 2.5 + }, + "prevControl": { + "x": 5.319684140380445, + "y": 2.5024264614921368 + }, + "nextControl": { + "x": 6.673958188657408, + "y": 2.4892814670138885 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.27038794626047, + "y": 3.5451864865548153 + }, + "prevControl": { + "x": 8.701588101689149, + "y": 0.9580746395575246 + }, + "nextControl": { + "x": 8.201584412594418, + "y": 3.9579934738012836 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.996921686746987, + "y": 3.1443915662650603 + }, + "prevControl": { + "x": 7.412174698795181, + "y": 4.297265060240964 + }, + "nextControl": { + "x": 6.855944174235065, + "y": 2.752993472317489 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.395203052662038, + "y": 2.342529188588278 + }, + "prevControl": { + "x": 3.9848846992289753, + "y": 2.3425291885882786 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.0, + "rotationDegrees": 135.0 + }, + { + "waypointRelativePos": 2.0, + "rotationDegrees": 135.0 + }, + { + "waypointRelativePos": 2.3571134868421066, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 2.7769325657894743, + "rotationDegrees": -96.86659894451316 + }, + { + "waypointRelativePos": 3.580468749999996, + "rotationDegrees": 68.48048252063953 + }, + { + "waypointRelativePos": 4.2, + "rotationDegrees": 45.0 + }, + { + "waypointRelativePos": 4.45, + "rotationDegrees": 45.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 4.4, + "maxWaypointRelativePos": 5.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 7.9, + "y": 3.2 + }, + "rotationOffset": 90.0, + "minWaypointRelativePos": 2.9292722176079757, + "maxWaypointRelativePos": 3.8679401993355524, + "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, + "rotation": 45.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.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 4fb9e60cb0..ce2f87f445 100644 --- a/src/main/java/frc/robot/autonomous/AutonomousConstants.java +++ b/src/main/java/frc/robot/autonomous/AutonomousConstants.java @@ -24,6 +24,7 @@ public class AutonomousConstants { public static final double TIME_TO_WAIT_TO_START_SHOOTING_AFTER_AUTO_START = 2.0; public static final double TIME_TO_WAIT_TO_START_WIGGLE_AFTER_PATH_END = 2.0; public static final double TIME_BETWEEN_WIGGLES_SECONDS = 0.3; + public static final double TIME_TO_EMPTY_TANK_AFTER_END_OF_PATH = 10; public static final double TIME_TO_WAIT_AT_DEPOT = 2; public static final Rotation2d WIGGLE_RANGE = Rotation2d.fromDegrees(5); diff --git a/src/main/java/frc/robot/autonomous/AutosBuilder.java b/src/main/java/frc/robot/autonomous/AutosBuilder.java index 2fb185350e..a59bc2d8c9 100644 --- a/src/main/java/frc/robot/autonomous/AutosBuilder.java +++ b/src/main/java/frc/robot/autonomous/AutosBuilder.java @@ -200,13 +200,17 @@ private static Supplier getQuarterAuto( .withDeadline(new WaitCommand(AutonomousConstants.TIME_TO_WAIT_AT_DEPOT)) ) .andThen( - getAllianceSideToStartingLineAuto( + getDoubleSwipeAuto( robot, - startingSide, + resetSubsystems, + openIntake, + closeIntake, + scoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, - stuckDebounceSeconds + stuckDebounceSeconds, + startingSide ) ) .asProxy(), @@ -248,4 +252,58 @@ private static Command getAllianceSideToStartingLineAuto( ); } + private static Command getDoubleSwipeAuto( + Robot robot, + Supplier resetSubsystems, + Supplier openIntake, + Supplier closeIntake, + Supplier scoreSequence, + PathConstraints pathfindingConstraints, + Pose2d regularIsNearEndOfPathTolerance, + Pose2d stuckIsNearEndOfPathTolerance, + double stuckDebounceSeconds, + AllianceSide allianceSide + ) { + return new ParallelCommandGroup( + PathFollowingCommandsBuilder + .followAdjustedPathThenStop( + robot.getSwerve(), + () -> robot.getPoseEstimator().getEstimatedPose(), + allianceSide == AllianceSide.DEPOT + ? PathHelper.PATH_PLANNER_PATHS.get("L double swipe") + : PathHelper.PATH_PLANNER_PATHS.get("R double swipe"), + 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()) + ) + ) + ) + ) + ); + } + }