From 1b4fcd2e1f99c8c56d3dfd97942fd1ac4f6d6cdd Mon Sep 17 00:00:00 2001 From: tomer-hershman Date: Mon, 16 Mar 2026 17:21:07 +0200 Subject: [PATCH 1/3] did the job --- .../robot/autonomous/AutonomousConstants.java | 1 + .../frc/robot/autonomous/AutosBuilder.java | 142 +++++++++++++----- 2 files changed, 108 insertions(+), 35 deletions(-) diff --git a/src/main/java/frc/robot/autonomous/AutonomousConstants.java b/src/main/java/frc/robot/autonomous/AutonomousConstants.java index 2dc7d349bf..81a2b39cf9 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 Rotation2d WIGGLE_RANGE = Rotation2d.fromDegrees(5); 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 9694149a92..efde4adc5c 100644 --- a/src/main/java/frc/robot/autonomous/AutosBuilder.java +++ b/src/main/java/frc/robot/autonomous/AutosBuilder.java @@ -74,45 +74,64 @@ private static Supplier getQuarterAuto( double stuckDebounceSeconds, AllianceSide startingSide ) { + hasPathEnded = false; return () -> new PathPlannerAutoWrapper( - new ParallelCommandGroup( - PathFollowingCommandsBuilder - .followAdjustedPathThenStop( - robot.getSwerve(), - () -> robot.getPoseEstimator().getEstimatedPose(), - startingSide == AllianceSide.DEPOT - ? PathHelper.PATH_PLANNER_PATHS.get("L quarter") - : PathHelper.PATH_PLANNER_PATHS.get("R quarter"), - 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) + new SequentialCommandGroup( + new ParallelDeadlineGroup( + new SequentialCommandGroup( + new WaitUntilCommand(() -> hasPathEnded), + new WaitCommand(AutonomousConstants.TIME_TO_EMPTY_TANK_AFTER_END_OF_PATH) + ), + new SequentialCommandGroup( + PathFollowingCommandsBuilder + .followAdjustedPathThenStop( + robot.getSwerve(), + () -> robot.getPoseEstimator().getEstimatedPose(), + startingSide == AllianceSide.DEPOT + ? PathHelper.PATH_PLANNER_PATHS.get("L quarter") + : PathHelper.PATH_PLANNER_PATHS.get("R quarter"), + pathfindingConstraints, + regularIsNearEndOfPathTolerance, + stuckIsNearEndOfPathTolerance, + stuckDebounceSeconds, + robot.getSwerve().getLogPath() + ) + .asProxy() + .alongWith(new InstantCommand(() -> hasPathEnded = false)) + .andThen(new InstantCommand(() -> hasPathEnded = true)), + new SequentialCommandGroup( + resetSubsystems.get(), + new ParallelDeadlineGroup( + 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()) ) - .asProxy(), - new WaitCommand(AutonomousConstants.TIME_TO_WAIT_TO_CLOSE_INTAKE_AFTER_PATH_END_SECONDS) - .andThen(closeIntake.get()) - ) + ) ) + ) ) + ), + doubleSwipeCommand( + robot, + resetSubsystems, + openIntake, + closeIntake, + scoreSequence, + pathfindingConstraints, + regularIsNearEndOfPathTolerance, + stuckIsNearEndOfPathTolerance, + stuckDebounceSeconds, + startingSide ) ), new Pose2d(), @@ -120,5 +139,58 @@ private static Supplier getQuarterAuto( ); } + private static Command doubleSwipeCommand( + 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()) + ) + ) + ) + ) + ); + } } From 613b5b9e3e89a417c00e2c25f759f554229dac40 Mon Sep 17 00:00:00 2001 From: tomer-hershman Date: Sun, 22 Mar 2026 11:51:33 +0200 Subject: [PATCH 2/3] did the job --- .../pathplanner/paths/L double swipe.path | 177 ++++++++++++++++++ .../pathplanner/paths/R double swipe.path | 172 +++++++++++++++++ 2 files changed, 349 insertions(+) create mode 100644 src/main/deploy/pathplanner/paths/L double swipe.path create mode 100644 src/main/deploy/pathplanner/paths/R double swipe.path 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 From 1866eb8737b5659a4d8160f9e101305c441c2c9c Mon Sep 17 00:00:00 2001 From: tomer-hershman Date: Sun, 22 Mar 2026 13:40:41 +0200 Subject: [PATCH 3/3] sA --- .../frc/robot/autonomous/AutosBuilder.java | 124 +++++++++--------- 1 file changed, 62 insertions(+), 62 deletions(-) diff --git a/src/main/java/frc/robot/autonomous/AutosBuilder.java b/src/main/java/frc/robot/autonomous/AutosBuilder.java index fedc7ccea0..a59bc2d8c9 100644 --- a/src/main/java/frc/robot/autonomous/AutosBuilder.java +++ b/src/main/java/frc/robot/autonomous/AutosBuilder.java @@ -201,16 +201,16 @@ private static Supplier getQuarterAuto( ) .andThen( getDoubleSwipeAuto( - robot, - resetSubsystems, - openIntake, - closeIntake, - scoreSequence, - pathfindingConstraints, - regularIsNearEndOfPathTolerance, - stuckIsNearEndOfPathTolerance, - stuckDebounceSeconds, - startingSide + robot, + resetSubsystems, + openIntake, + closeIntake, + scoreSequence, + pathfindingConstraints, + regularIsNearEndOfPathTolerance, + stuckIsNearEndOfPathTolerance, + stuckDebounceSeconds, + startingSide ) ) .asProxy(), @@ -252,58 +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() + 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()) - ) - ) - ) - ) - ); - } + ) + .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()) + ) + ) + ) + ) + ); + } }