From c8b80a5c1581427f1eaaeacb50fd63ecc47daf0f Mon Sep 17 00:00:00 2001 From: drorta <125767200+drorta@users.noreply.github.com> Date: Sat, 14 Mar 2026 21:06:09 +0200 Subject: [PATCH 1/9] aa --- src/main/java/frc/robot/Robot.java | 3 + .../robot/autonomous/AutonomousConstants.java | 2 + .../frc/robot/autonomous/AutosBuilder.java | 75 +++++++++++++++++++ 3 files changed, 80 insertions(+) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7669a7f2b4..5f6f2a5c50 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -363,6 +363,8 @@ private void configureAuto() { Supplier autonomousResetSubsystemsCommand = () -> getRobotCommander().setState(RobotState.RESET_SUBSYSTEMS); + Supplier autonomousPassingSequenceCommand = () -> getRobotCommander().passSequence(); + getSwerve().configPathPlanner(() -> getPoseEstimator().getEstimatedPose(), (pose) -> {}, getRobotConfig()); this.autonomousChooser = new AutonomousChooser( @@ -373,6 +375,7 @@ private void configureAuto() { autonomousOpenIntakeCommand, autonomousCloseIntakeCommand, autonomousScoringSequenceCommand, + autonomousPassingSequenceCommand, AutonomousConstants.DEFAULT_PATHFINDING_CONSTRAINTS, AutonomousConstants.DEFAULT_IS_NEAR_END_OF_PATH_TOLERANCE, AutonomousConstants.DEFAULT_STUCK_IS_NEAR_END_OF_PATH_TOLERANCE, diff --git a/src/main/java/frc/robot/autonomous/AutonomousConstants.java b/src/main/java/frc/robot/autonomous/AutonomousConstants.java index 2dc7d349bf..935a618161 100644 --- a/src/main/java/frc/robot/autonomous/AutonomousConstants.java +++ b/src/main/java/frc/robot/autonomous/AutonomousConstants.java @@ -22,7 +22,9 @@ public class AutonomousConstants { public static final double TIME_TO_WAIT_TO_CLOSE_INTAKE_AFTER_PATH_END_SECONDS = 4.0; public static final double TIME_TO_WAIT_TO_START_SHOOTING_AFTER_AUTO_START = 2.0; + public static final double TIME_TO_WAIT_TO_START_PASSING_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_TO_WAIT_TO_START_SHOOTING_AFTER_PASSING = 3.0; public static final double TIME_BETWEEN_WIGGLES_SECONDS = 0.3; 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 9694149a92..87602891e2 100644 --- a/src/main/java/frc/robot/autonomous/AutosBuilder.java +++ b/src/main/java/frc/robot/autonomous/AutosBuilder.java @@ -29,6 +29,7 @@ public static List> getAutoList( Supplier openIntake, Supplier closeIntake, Supplier scoreSequence, + Supplier passingSequence, PathConstraints pathfindingConstraints, Pose2d regularIsNearEndOfPathTolerance, Pose2d stuckIsNearEndOfPathTolerance, @@ -58,6 +59,18 @@ public static List> getAutoList( stuckIsNearEndOfPathTolerance, stuckDebounceSeconds, AllianceSide.DEPOT + ), + getRQuarterPassingAuto( + robot, + resetSubsystems, + openIntake, + closeIntake, + scoreSequence, + passingSequence, + pathfindingConstraints, + regularIsNearEndOfPathTolerance, + stuckIsNearEndOfPathTolerance, + stuckDebounceSeconds ) ); } @@ -120,5 +133,67 @@ private static Supplier getQuarterAuto( ); } + private static Supplier getRQuarterPassingAuto( + Robot robot, + Supplier resetSubsystems, + Supplier openIntake, + Supplier closeIntake, + Supplier scoreSequence, + Supplier passingSequence, + PathConstraints pathfindingConstraints, + Pose2d regularIsNearEndOfPathTolerance, + Pose2d stuckIsNearEndOfPathTolerance, + double stuckDebounceSeconds + ) { + return () -> new PathPlannerAutoWrapper( + new ParallelCommandGroup( + PathFollowingCommandsBuilder + .followAdjustedPathThenStop( + robot.getSwerve(), + () -> robot.getPoseEstimator().getEstimatedPose(), + PathHelper.PATH_PLANNER_PATHS.get("R quarter passing"), + 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 SequentialCommandGroup( + new WaitCommand(AutonomousConstants.TIME_TO_WAIT_TO_START_PASSING_AFTER_AUTO_START), + new ParallelDeadlineGroup( + new WaitCommand(AutonomousConstants.TIME_TO_WAIT_TO_START_SHOOTING_AFTER_PASSING), + passingSequence.get() + ), + 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(), + "R quarter passing" + ); + } + } From 0678cb45156de20cdd969e1d8bb031034afd4505 Mon Sep 17 00:00:00 2001 From: drorta <125767200+drorta@users.noreply.github.com> Date: Sun, 15 Mar 2026 12:07:26 +0200 Subject: [PATCH 2/9] path --- .../pathplanner/paths/R half passing.path | 161 ++++++++++++++++++ 1 file changed, 161 insertions(+) create mode 100644 src/main/deploy/pathplanner/paths/R half passing.path diff --git a/src/main/deploy/pathplanner/paths/R half passing.path b/src/main/deploy/pathplanner/paths/R half passing.path new file mode 100644 index 0000000000..6f6fddd889 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/R half passing.path @@ -0,0 +1,161 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.395203052662038, + "y": 2.5 + }, + "prevControl": null, + "nextControl": { + "x": 4.6936982492178245, + "y": 2.521745650604434 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.875901051347584, + "y": 2.5 + }, + "prevControl": { + "x": 5.62851680559387, + "y": 2.536069862113212 + }, + "nextControl": { + "x": 6.3981325873247705, + "y": 2.423856026334752 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.953260436028501, + "y": 2.125349819934945 + }, + "prevControl": { + "x": 6.704863771234131, + "y": 2.1536181223076314 + }, + "nextControl": { + "x": 7.20165710082287, + "y": 2.097081517562259 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.853981954770756, + "y": 2.125349819934945 + }, + "prevControl": { + "x": 7.2659071456461275, + "y": 2.196210918292518 + }, + "nextControl": { + "x": 8.889739534928747, + "y": 2.000544406366172 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.768882047707558, + "y": 2.865055374845107 + }, + "prevControl": { + "x": 8.758357313549103, + "y": 2.6152770131503957 + }, + "nextControl": { + "x": 8.779406781866014, + "y": 3.114833736539818 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.60322495546778, + "y": 3.8054259603469642 + }, + "prevControl": { + "x": 9.074740212592936, + "y": 4.143079790117721 + }, + "nextControl": { + "x": 7.3595574456580986, + "y": 3.7495140236683864 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.378702563506815, + "y": 2.865055374845107 + }, + "prevControl": { + "x": 7.550889928985488, + "y": 3.0463052332437104 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.99, + "rotationDegrees": 135.0 + }, + { + "waypointRelativePos": 2.95012454710145, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 4.0, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 2.2706213662790695, + "maxWaypointRelativePos": 5.0, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 300.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": 89.65863899757802 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 135.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file From 4a0ddcce9fa51d2160e46f514d4d3f17c2a12a23 Mon Sep 17 00:00:00 2001 From: drorta <125767200+drorta@users.noreply.github.com> Date: Sun, 15 Mar 2026 14:58:20 +0200 Subject: [PATCH 3/9] aa --- .../pathplanner/paths/R half passing.path | 47 ++++++++++++------- 1 file changed, 29 insertions(+), 18 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/R half passing.path b/src/main/deploy/pathplanner/paths/R half passing.path index 6f6fddd889..7052e89ee0 100644 --- a/src/main/deploy/pathplanner/paths/R half passing.path +++ b/src/main/deploy/pathplanner/paths/R half passing.path @@ -64,44 +64,44 @@ }, { "anchor": { - "x": 8.768882047707558, - "y": 2.865055374845107 + "x": 8.76845753949814, + "y": 2.890186260842628 }, "prevControl": { - "x": 8.758357313549103, - "y": 2.6152770131503957 + "x": 8.784997612235493, + "y": 2.640734008910834 }, "nextControl": { - "x": 8.779406781866014, - "y": 3.114833736539818 + "x": 8.732491216193727, + "y": 3.432619204425258 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.60322495546778, - "y": 3.8054259603469642 + "x": 7.517431846344486, + "y": 3.7873843614467164 }, "prevControl": { - "x": 9.074740212592936, - "y": 4.143079790117721 + "x": 8.988947103469636, + "y": 4.125038191217473 }, "nextControl": { - "x": 7.3595574456580986, - "y": 3.7495140236683864 + "x": 7.273764336534804, + "y": 3.7314724247681386 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.378702563506815, - "y": 2.865055374845107 + "x": 6.798951701905204, + "y": 3.1991433356567542 }, "prevControl": { - "x": 7.550889928985488, - "y": 3.0463052332437104 + "x": 6.980324711280639, + "y": 3.371200975621093 }, "nextControl": null, "isLocked": false, @@ -137,7 +137,18 @@ } } ], - "pointTowardsZones": [], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 8.2, + "y": 3.2 + }, + "rotationOffset": 90.0, + "minWaypointRelativePos": 4.0236191860465125, + "maxWaypointRelativePos": 5.199606897463004, + "name": "Point Towards Zone" + } + ], "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.2, @@ -149,7 +160,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 89.65863899757802 + "rotation": 66.76690089013526 }, "reversed": false, "folder": null, From b7c26e08991b0e218e357648ff55892069d02812 Mon Sep 17 00:00:00 2001 From: drorta <125767200+drorta@users.noreply.github.com> Date: Tue, 17 Mar 2026 16:27:08 +0200 Subject: [PATCH 4/9] path --- .../pathplanner/paths/R half passing.path | 177 ++++++++++++++++++ 1 file changed, 177 insertions(+) diff --git a/src/main/deploy/pathplanner/paths/R half passing.path b/src/main/deploy/pathplanner/paths/R half passing.path index 7052e89ee0..78994c8adf 100644 --- a/src/main/deploy/pathplanner/paths/R half passing.path +++ b/src/main/deploy/pathplanner/paths/R half passing.path @@ -64,6 +64,7 @@ }, { "anchor": { +<<<<<<< Updated upstream "x": 8.76845753949814, "y": 2.890186260842628 }, @@ -74,6 +75,18 @@ "nextControl": { "x": 8.732491216193727, "y": 3.432619204425258 +======= + "x": 8.71051216891264, + "y": 2.954287000464685 + }, + "prevControl": { + "x": 8.749313992967286, + "y": 2.707316519536229 + }, + "nextControl": { + "x": 8.626289740164188, + "y": 3.4903559673172255 +>>>>>>> Stashed changes }, "isLocked": false, "linkedName": null @@ -103,6 +116,121 @@ "x": 6.980324711280639, "y": 3.371200975621093 }, +<<<<<<< Updated upstream +======= + "nextControl": { + "x": 6.617578692529769, + "y": 3.0270856956924153 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.875901051347584, + "y": 2.7329908708952915 + }, + "prevControl": { + "x": 6.520950949811972, + "y": 2.815194654305315 + }, + "nextControl": { + "x": 5.230851152883195, + "y": 2.650787087485268 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.395203052662038, + "y": 2.7329908708952915 + }, + "prevControl": { + "x": 4.113343590533785, + "y": 2.743221518742256 + }, + "nextControl": { + "x": 2.6770625147902907, + "y": 2.722760223048327 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.5756613518432465, + "y": 2.5 + }, + "prevControl": { + "x": 0.9128909857486149, + "y": 2.553445583565675 + }, + "nextControl": { + "x": 0.23843171793787804, + "y": 2.446554416434325 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.066024773466543, + "y": 1.6619142077137554 + }, + "prevControl": { + "x": 1.3954701409262886, + "y": 2.332561088793362 + }, + "nextControl": { + "x": 2.374599790892193, + "y": 1.353296739467163 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.7214374709572491, + "y": 1.5211897362918227 + }, + "prevControl": { + "x": 1.6031972149911748, + "y": 1.896470983800263 + }, + "nextControl": { + "x": 0.49140485950698964, + "y": 1.423286734962068 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.066024773466543, + "y": 0.8233406908302368 + }, + "prevControl": { + "x": 0.7577187786816997, + "y": 1.0345586089662913 + }, + "nextControl": { + "x": 3.0562750735749686, + "y": 0.6634708991635694 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.5756613518432465, + "y": 0.6403776525712521 + }, + "prevControl": { + "x": 2.0097349848977695, + "y": 0.4934129104708813 + }, +>>>>>>> Stashed changes "nextControl": null, "isLocked": false, "linkedName": null @@ -114,23 +242,53 @@ "rotationDegrees": 135.0 }, { +<<<<<<< Updated upstream "waypointRelativePos": 2.95012454710145, "rotationDegrees": 180.0 }, { "waypointRelativePos": 4.0, "rotationDegrees": -90.0 +======= + "waypointRelativePos": 2.9180041326992754, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 7.0, + "rotationDegrees": 45.0 + }, + { + "waypointRelativePos": 8.007536514945645, + "rotationDegrees": 45.0 + }, + { + "waypointRelativePos": 9.0009765625, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 10.995612545289848, + "rotationDegrees": 0.0 +>>>>>>> Stashed changes } ], "constraintZones": [ { "name": "Constraints Zone", +<<<<<<< Updated upstream "minWaypointRelativePos": 2.2706213662790695, "maxWaypointRelativePos": 5.0, "constraints": { "maxVelocity": 1.0, "maxAcceleration": 2.0, "maxAngularVelocity": 300.0, +======= + "minWaypointRelativePos": 1.5718485729386888, + "maxWaypointRelativePos": 6.448161667547567, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 150.0, +>>>>>>> Stashed changes "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false @@ -144,8 +302,23 @@ "y": 3.2 }, "rotationOffset": 90.0, +<<<<<<< Updated upstream "minWaypointRelativePos": 4.0236191860465125, "maxWaypointRelativePos": 5.199606897463004, +======= + "minWaypointRelativePos": 3.9905688424947137, + "maxWaypointRelativePos": 5.141087473572938, + "name": "Point Towards Zone" + }, + { + "fieldPosition": { + "x": 8.2, + "y": 2.7 + }, + "rotationOffset": 90.0, + "minWaypointRelativePos": 3.081907373150104, + "maxWaypointRelativePos": 3.884629360465116, +>>>>>>> Stashed changes "name": "Point Towards Zone" } ], @@ -160,7 +333,11 @@ }, "goalEndState": { "velocity": 0, +<<<<<<< Updated upstream "rotation": 66.76690089013526 +======= + "rotation": 0.0 +>>>>>>> Stashed changes }, "reversed": false, "folder": null, From 46b07ac6fc0e1f1a533b715ff85665b4dd88826a Mon Sep 17 00:00:00 2001 From: drorta <125767200+drorta@users.noreply.github.com> Date: Tue, 17 Mar 2026 16:27:57 +0200 Subject: [PATCH 5/9] aa --- .../pathplanner/paths/R half passing.path | 43 ------------------- 1 file changed, 43 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/R half passing.path b/src/main/deploy/pathplanner/paths/R half passing.path index 78994c8adf..a6e9df07f0 100644 --- a/src/main/deploy/pathplanner/paths/R half passing.path +++ b/src/main/deploy/pathplanner/paths/R half passing.path @@ -64,18 +64,6 @@ }, { "anchor": { -<<<<<<< Updated upstream - "x": 8.76845753949814, - "y": 2.890186260842628 - }, - "prevControl": { - "x": 8.784997612235493, - "y": 2.640734008910834 - }, - "nextControl": { - "x": 8.732491216193727, - "y": 3.432619204425258 -======= "x": 8.71051216891264, "y": 2.954287000464685 }, @@ -86,7 +74,6 @@ "nextControl": { "x": 8.626289740164188, "y": 3.4903559673172255 ->>>>>>> Stashed changes }, "isLocked": false, "linkedName": null @@ -116,8 +103,6 @@ "x": 6.980324711280639, "y": 3.371200975621093 }, -<<<<<<< Updated upstream -======= "nextControl": { "x": 6.617578692529769, "y": 3.0270856956924153 @@ -230,7 +215,6 @@ "x": 2.0097349848977695, "y": 0.4934129104708813 }, ->>>>>>> Stashed changes "nextControl": null, "isLocked": false, "linkedName": null @@ -242,14 +226,6 @@ "rotationDegrees": 135.0 }, { -<<<<<<< Updated upstream - "waypointRelativePos": 2.95012454710145, - "rotationDegrees": 180.0 - }, - { - "waypointRelativePos": 4.0, - "rotationDegrees": -90.0 -======= "waypointRelativePos": 2.9180041326992754, "rotationDegrees": 180.0 }, @@ -268,27 +244,17 @@ { "waypointRelativePos": 10.995612545289848, "rotationDegrees": 0.0 ->>>>>>> Stashed changes } ], "constraintZones": [ { "name": "Constraints Zone", -<<<<<<< Updated upstream - "minWaypointRelativePos": 2.2706213662790695, - "maxWaypointRelativePos": 5.0, - "constraints": { - "maxVelocity": 1.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 300.0, -======= "minWaypointRelativePos": 1.5718485729386888, "maxWaypointRelativePos": 6.448161667547567, "constraints": { "maxVelocity": 1.0, "maxAcceleration": 2.0, "maxAngularVelocity": 150.0, ->>>>>>> Stashed changes "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false @@ -302,10 +268,6 @@ "y": 3.2 }, "rotationOffset": 90.0, -<<<<<<< Updated upstream - "minWaypointRelativePos": 4.0236191860465125, - "maxWaypointRelativePos": 5.199606897463004, -======= "minWaypointRelativePos": 3.9905688424947137, "maxWaypointRelativePos": 5.141087473572938, "name": "Point Towards Zone" @@ -318,7 +280,6 @@ "rotationOffset": 90.0, "minWaypointRelativePos": 3.081907373150104, "maxWaypointRelativePos": 3.884629360465116, ->>>>>>> Stashed changes "name": "Point Towards Zone" } ], @@ -333,11 +294,7 @@ }, "goalEndState": { "velocity": 0, -<<<<<<< Updated upstream - "rotation": 66.76690089013526 -======= "rotation": 0.0 ->>>>>>> Stashed changes }, "reversed": false, "folder": null, From 5ed94efc0657da785835544bea55c8086de807ef Mon Sep 17 00:00:00 2001 From: greenblitz4590 <94316058+greenblitz4590@users.noreply.github.com> Date: Sun, 22 Mar 2026 16:38:57 +0200 Subject: [PATCH 6/9] code --- .../pathplanner/paths/Depot-to-Outpost.path | 4 +-- .../paths/Outpost - Starting line.path | 2 +- .../pathplanner/paths/Outpost-to-Depot.path | 4 +-- .../pathplanner/paths/R half passing.path | 29 ++++++++++++++----- .../deploy/pathplanner/paths/R quarter.path | 2 +- src/main/java/frc/RobotManager.java | 10 ++++--- .../frc/robot/autonomous/AutosBuilder.java | 2 +- 7 files changed, 34 insertions(+), 19 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Depot-to-Outpost.path b/src/main/deploy/pathplanner/paths/Depot-to-Outpost.path index 1f9d069b7c..efd4dbeeaf 100644 --- a/src/main/deploy/pathplanner/paths/Depot-to-Outpost.path +++ b/src/main/deploy/pathplanner/paths/Depot-to-Outpost.path @@ -12,7 +12,7 @@ "y": 3.6901959675661757 }, "isLocked": false, - "linkedName": "Depot" + "linkedName": null }, { "anchor": { @@ -25,7 +25,7 @@ }, "nextControl": null, "isLocked": false, - "linkedName": "Outpost" + "linkedName": null } ], "rotationTargets": [], diff --git a/src/main/deploy/pathplanner/paths/Outpost - Starting line.path b/src/main/deploy/pathplanner/paths/Outpost - Starting line.path index 7534df1f7e..77c79304ed 100644 --- a/src/main/deploy/pathplanner/paths/Outpost - Starting line.path +++ b/src/main/deploy/pathplanner/paths/Outpost - Starting line.path @@ -12,7 +12,7 @@ "y": 1.0471295424792166 }, "isLocked": false, - "linkedName": null + "linkedName": "Outpost" }, { "anchor": { diff --git a/src/main/deploy/pathplanner/paths/Outpost-to-Depot.path b/src/main/deploy/pathplanner/paths/Outpost-to-Depot.path index fdfa3fbcd2..8d38db9565 100644 --- a/src/main/deploy/pathplanner/paths/Outpost-to-Depot.path +++ b/src/main/deploy/pathplanner/paths/Outpost-to-Depot.path @@ -12,7 +12,7 @@ "y": 3.797670514229443 }, "isLocked": false, - "linkedName": "Outpost" + "linkedName": null }, { "anchor": { @@ -25,7 +25,7 @@ }, "nextControl": null, "isLocked": false, - "linkedName": "Depot" + "linkedName": null } ], "rotationTargets": [], diff --git a/src/main/deploy/pathplanner/paths/R half passing.path b/src/main/deploy/pathplanner/paths/R half passing.path index a6e9df07f0..50cd4a8ef6 100644 --- a/src/main/deploy/pathplanner/paths/R half passing.path +++ b/src/main/deploy/pathplanner/paths/R half passing.path @@ -144,15 +144,15 @@ }, { "anchor": { - "x": 0.5756613518432465, + "x": 0.643, "y": 2.5 }, "prevControl": { - "x": 0.9128909857486149, + "x": 0.9802296339053684, "y": 2.553445583565675 }, "nextControl": { - "x": 0.23843171793787804, + "x": 0.3057703660946315, "y": 2.446554416434325 }, "isLocked": false, @@ -208,16 +208,16 @@ }, { "anchor": { - "x": 0.5756613518432465, - "y": 0.6403776525712521 + "x": 0.643, + "y": 0.736 }, "prevControl": { - "x": 2.0097349848977695, - "y": 0.4934129104708813 + "x": 2.077073633054523, + "y": 0.5890352578996292 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "Outpost" } ], "rotationTargets": [ @@ -259,6 +259,19 @@ "nominalVoltage": 12.0, "unlimited": false } + }, + { + "name": "Just for test", + "minWaypointRelativePos": 7.958472873409204, + "maxWaypointRelativePos": 13.0, + "constraints": { + "maxVelocity": 0.5, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } } ], "pointTowardsZones": [ diff --git a/src/main/deploy/pathplanner/paths/R quarter.path b/src/main/deploy/pathplanner/paths/R quarter.path index c104be027c..99c945013a 100644 --- a/src/main/deploy/pathplanner/paths/R quarter.path +++ b/src/main/deploy/pathplanner/paths/R quarter.path @@ -105,7 +105,7 @@ }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "Outpost" } ], "rotationTargets": [ diff --git a/src/main/java/frc/RobotManager.java b/src/main/java/frc/RobotManager.java index 6131556262..d3e32f64e1 100644 --- a/src/main/java/frc/RobotManager.java +++ b/src/main/java/frc/RobotManager.java @@ -8,6 +8,8 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Threads; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.Robot; import frc.utils.GamePeriodUtils; import frc.utils.HubUtil; @@ -49,10 +51,10 @@ public RobotManager() { JoysticksBindings.configureBindings(robot); Threads.setCurrentThreadPriority(true, 10); - - robot.getAutonomousChooser().getChooser().onChange((autonomousCommand) -> { - this.autonomousCommand = autonomousCommand.get(); - }); + CommandScheduler.getInstance().schedule(new WaitCommand(1).andThen(new InstantCommand(() -> robot.getAutonomousChooser().getChooser().onChange((autonomousCommand) -> { + this.autonomousCommand = autonomousCommand.get(); + })) + )); } @Override diff --git a/src/main/java/frc/robot/autonomous/AutosBuilder.java b/src/main/java/frc/robot/autonomous/AutosBuilder.java index 19bbaf9e4f..5209e79b2c 100644 --- a/src/main/java/frc/robot/autonomous/AutosBuilder.java +++ b/src/main/java/frc/robot/autonomous/AutosBuilder.java @@ -161,7 +161,7 @@ private static Supplier getRQuarterPassingAuto( .followAdjustedPathThenStop( robot.getSwerve(), () -> robot.getPoseEstimator().getEstimatedPose(), - PathHelper.PATH_PLANNER_PATHS.get("R quarter passing"), + PathHelper.PATH_PLANNER_PATHS.get("R half passing"), pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, From 02b8336804f71f290e644e0988bc18bd46adfda7 Mon Sep 17 00:00:00 2001 From: itamaroryan Date: Sun, 22 Mar 2026 19:47:00 +0200 Subject: [PATCH 7/9] code --- .../deploy/pathplanner/paths/L quarter.path | 2 +- .../paths/Outpost - Starting line.path | 4 +-- .../pathplanner/paths/R half passing.path | 25 +++++-------------- .../deploy/pathplanner/paths/R quarter.path | 4 +-- src/main/java/frc/RobotManager.java | 9 ++++--- 5 files changed, 16 insertions(+), 28 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/L quarter.path b/src/main/deploy/pathplanner/paths/L quarter.path index c3adc3820e..11d3ca9479 100644 --- a/src/main/deploy/pathplanner/paths/L quarter.path +++ b/src/main/deploy/pathplanner/paths/L quarter.path @@ -12,7 +12,7 @@ "y": 5.695287264914479 }, "isLocked": false, - "linkedName": null + "linkedName": "L starting" }, { "anchor": { diff --git a/src/main/deploy/pathplanner/paths/Outpost - Starting line.path b/src/main/deploy/pathplanner/paths/Outpost - Starting line.path index 77c79304ed..023a9a4779 100644 --- a/src/main/deploy/pathplanner/paths/Outpost - Starting line.path +++ b/src/main/deploy/pathplanner/paths/Outpost - Starting line.path @@ -4,12 +4,12 @@ { "anchor": { "x": 0.643, - "y": 0.736 + "y": 5.973 }, "prevControl": null, "nextControl": { "x": 1.410966233360837, - "y": 1.0471295424792166 + "y": 6.284129542479217 }, "isLocked": false, "linkedName": "Outpost" diff --git a/src/main/deploy/pathplanner/paths/R half passing.path b/src/main/deploy/pathplanner/paths/R half passing.path index 50cd4a8ef6..56c7b506ef 100644 --- a/src/main/deploy/pathplanner/paths/R half passing.path +++ b/src/main/deploy/pathplanner/paths/R half passing.path @@ -144,15 +144,15 @@ }, { "anchor": { - "x": 0.643, + "x": 0.7, "y": 2.5 }, "prevControl": { - "x": 0.9802296339053684, + "x": 1.0372296339053684, "y": 2.553445583565675 }, "nextControl": { - "x": 0.3057703660946315, + "x": 0.3627703660946314, "y": 2.446554416434325 }, "isLocked": false, @@ -212,12 +212,12 @@ "y": 0.736 }, "prevControl": { - "x": 2.077073633054523, - "y": 0.5890352578996292 + "x": 2.0770736330545216, + "y": 0.5890352578996298 }, "nextControl": null, "isLocked": false, - "linkedName": "Outpost" + "linkedName": null } ], "rotationTargets": [ @@ -259,19 +259,6 @@ "nominalVoltage": 12.0, "unlimited": false } - }, - { - "name": "Just for test", - "minWaypointRelativePos": 7.958472873409204, - "maxWaypointRelativePos": 13.0, - "constraints": { - "maxVelocity": 0.5, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } } ], "pointTowardsZones": [ diff --git a/src/main/deploy/pathplanner/paths/R quarter.path b/src/main/deploy/pathplanner/paths/R quarter.path index 99c945013a..495585ba4f 100644 --- a/src/main/deploy/pathplanner/paths/R quarter.path +++ b/src/main/deploy/pathplanner/paths/R quarter.path @@ -97,11 +97,11 @@ { "anchor": { "x": 0.643, - "y": 0.736 + "y": 5.973 }, "prevControl": { "x": 1.2326816465669375, - "y": 0.7360000000000001 + "y": 5.973 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc/RobotManager.java b/src/main/java/frc/RobotManager.java index d3e32f64e1..de0230b761 100644 --- a/src/main/java/frc/RobotManager.java +++ b/src/main/java/frc/RobotManager.java @@ -51,10 +51,11 @@ public RobotManager() { JoysticksBindings.configureBindings(robot); Threads.setCurrentThreadPriority(true, 10); - CommandScheduler.getInstance().schedule(new WaitCommand(1).andThen(new InstantCommand(() -> robot.getAutonomousChooser().getChooser().onChange((autonomousCommand) -> { - this.autonomousCommand = autonomousCommand.get(); - })) - )); + robot.getAutonomousChooser().getChooser().onChange((autonomousCommand) -> { + this.autonomousCommand = autonomousCommand.get(); + }); +// CommandScheduler.getInstance().schedule(new WaitCommand(1).andThen(new InstantCommand(() -> +// )); } @Override From 24fd7e52eb8bf429e162492b5d3cfa00efbd5fde Mon Sep 17 00:00:00 2001 From: itamaroryan Date: Sun, 22 Mar 2026 19:52:18 +0200 Subject: [PATCH 8/9] add --- .../pathplanner/paths/L half passing.path | 278 ++++++++++++++++++ 1 file changed, 278 insertions(+) create mode 100644 src/main/deploy/pathplanner/paths/L half passing.path diff --git a/src/main/deploy/pathplanner/paths/L half passing.path b/src/main/deploy/pathplanner/paths/L half passing.path new file mode 100644 index 0000000000..6aad46f794 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/L half passing.path @@ -0,0 +1,278 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.395203052662038, + "y": 5.71 + }, + "prevControl": null, + "nextControl": { + "x": 4.6936982492178245, + "y": 5.68 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.875901051347584, + "y": 5.71 + }, + "prevControl": { + "x": 5.62851680559387, + "y": 5.67 + }, + "nextControl": { + "x": 6.3981325873247705, + "y": 5.65 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.911440798858774, + "y": 5.973 + }, + "prevControl": { + "x": 6.663044134064404, + "y": 6.023 + }, + "nextControl": { + "x": 7.159837463653143, + "y": 5.872999999999999 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.282938659058486, + "y": 6.1375320970042795 + }, + "prevControl": { + "x": 7.694863849933857, + "y": 6.337532097004279 + }, + "nextControl": { + "x": 9.318696239216477, + "y": 5.837532097004279 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.813423680452932, + "y": 4.714279600570615 + }, + "prevControl": { + "x": 8.861038144372026, + "y": 4.959703443800439 + }, + "nextControl": { + "x": 8.72920125170448, + "y": 4.414279600570614 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.545435092721117, + "y": 4.351997146936273 + }, + "prevControl": { + "x": 7.783197585165418, + "y": 4.274737861496438 + }, + "nextControl": { + "x": 7.294938829716058, + "y": 4.4333941834753885 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.545435092721117, + "y": 5.309457917261056 + }, + "prevControl": { + "x": 7.528314099142832, + "y": 5.035522020046176 + }, + "nextControl": { + "x": 7.571312410841653, + "y": 5.723495007132667 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.395203052662038, + "y": 5.548 + }, + "prevControl": { + "x": 4.113343590533785, + "y": 5.498 + }, + "nextControl": { + "x": 2.6770625147902907, + "y": 5.598 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.643, + "y": 4.882482168334275 + }, + "prevControl": { + "x": 0.6931106826697202, + "y": 4.545268963571586 + }, + "nextControl": { + "x": 0.606252927687199, + "y": 5.129766726437764 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.066024773466543, + "y": 5.548 + }, + "prevControl": { + "x": 1.3954701409262886, + "y": 4.877353118920394 + }, + "nextControl": { + "x": 2.374599790892193, + "y": 5.856617468246593 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.643, + "y": 5.973 + }, + "prevControl": { + "x": 1.5247597440339258, + "y": 5.597718752491559 + }, + "nextControl": { + "x": 0.41296738854974047, + "y": 6.0709030013297545 + }, + "isLocked": false, + "linkedName": "Depot" + }, + { + "anchor": { + "x": 0.643, + "y": 7.133808844507846 + }, + "prevControl": { + "x": 4.058805991440798, + "y": 7.392582025677604 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.9381663113006262, + "rotationDegrees": -135.0 + }, + { + "waypointRelativePos": 1.7590618336886934, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 3.2132196162046975, + "rotationDegrees": 125.00000000000001 + }, + { + "waypointRelativePos": 4.104477611940269, + "rotationDegrees": 20.0 + }, + { + "waypointRelativePos": 5.441364605543682, + "rotationDegrees": -119.99999999999999 + }, + { + "waypointRelativePos": 6.426439232409365, + "rotationDegrees": -135.0 + }, + { + "waypointRelativePos": 6.989339019189722, + "rotationDegrees": -135.0 + }, + { + "waypointRelativePos": 7.5053304904050915, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 10.01492537313423, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 7.189736664415911, + "maxWaypointRelativePos": 11.0, + "constraints": { + "maxVelocity": 1.2, + "maxAcceleration": 3, + "maxAngularVelocity": 540, + "maxAngularAcceleration": 720, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.8122889939230367, + "maxWaypointRelativePos": 6.2687373396353925, + "constraints": { + "maxVelocity": 1.3, + "maxAcceleration": 3, + "maxAngularVelocity": 540, + "maxAngularAcceleration": 720, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3, + "maxAcceleration": 3, + "maxAngularVelocity": 540, + "maxAngularAcceleration": 720, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -135.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file From 53e042f8ede3db1ff08ec0f5d2fabce3d2ebbd45 Mon Sep 17 00:00:00 2001 From: itamaroryan Date: Sun, 22 Mar 2026 19:58:58 +0200 Subject: [PATCH 9/9] full code unchecked --- .../frc/robot/autonomous/AutosBuilder.java | 27 ++++++++++++++----- 1 file changed, 21 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/autonomous/AutosBuilder.java b/src/main/java/frc/robot/autonomous/AutosBuilder.java index 5209e79b2c..029635dc70 100644 --- a/src/main/java/frc/robot/autonomous/AutosBuilder.java +++ b/src/main/java/frc/robot/autonomous/AutosBuilder.java @@ -60,7 +60,7 @@ public static List> getAutoList( stuckDebounceSeconds, AllianceSide.DEPOT ), - getRQuarterPassingAuto( + getQuarterPassingAuto( robot, resetSubsystems, openIntake, @@ -70,7 +70,21 @@ public static List> getAutoList( pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, - stuckDebounceSeconds + stuckDebounceSeconds, + AllianceSide.OUTPOST + ), + getQuarterPassingAuto( + robot, + resetSubsystems, + openIntake, + closeIntake, + scoreSequence, + passingSequence, + pathfindingConstraints, + regularIsNearEndOfPathTolerance, + stuckIsNearEndOfPathTolerance, + stuckDebounceSeconds, + AllianceSide.DEPOT ) ); } @@ -143,7 +157,7 @@ private static Supplier getQuarterAuto( ); } - private static Supplier getRQuarterPassingAuto( + private static Supplier getQuarterPassingAuto( Robot robot, Supplier resetSubsystems, Supplier openIntake, @@ -153,7 +167,8 @@ private static Supplier getRQuarterPassingAuto( PathConstraints pathfindingConstraints, Pose2d regularIsNearEndOfPathTolerance, Pose2d stuckIsNearEndOfPathTolerance, - double stuckDebounceSeconds + double stuckDebounceSeconds, + AllianceSide startingSide ) { return () -> new PathPlannerAutoWrapper( new ParallelCommandGroup( @@ -161,7 +176,7 @@ private static Supplier getRQuarterPassingAuto( .followAdjustedPathThenStop( robot.getSwerve(), () -> robot.getPoseEstimator().getEstimatedPose(), - PathHelper.PATH_PLANNER_PATHS.get("R half passing"), + startingSide == AllianceSide.OUTPOST ? PathHelper.PATH_PLANNER_PATHS.get("R half passing") : PathHelper.PATH_PLANNER_PATHS.get("L half passing"), pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -201,7 +216,7 @@ private static Supplier getRQuarterPassingAuto( ) ), new Pose2d(), - "R quarter passing" + startingSide == AllianceSide.OUTPOST ? "R quarter passing" : "L quarter passing" ); }