From c6268383e500a3c10e54e4c7da3d6f70eb4902d0 Mon Sep 17 00:00:00 2001 From: itamaroryan Date: Wed, 1 Jul 2026 16:54:52 +0300 Subject: [PATCH 1/8] nitay - codde --- .../pathplanner/paths/Depot - Middle.path | 34 +-- .../paths/Depot - Starting line.path | 4 +- .../deploy/pathplanner/paths/L High Tide.path | 161 ++++++++++ .../deploy/pathplanner/paths/L horseshoe.path | 10 +- .../pathplanner/paths/Outpost - Middle.path | 34 +-- .../deploy/pathplanner/paths/R High Tide.path | 177 +++++++++++ .../pathplanner/paths/R quarter light.path | 10 +- .../deploy/pathplanner/paths/R quarter.path | 10 +- .../robot/autonomous/AutonomousConstants.java | 1 + .../frc/robot/autonomous/AutosBuilder.java | 280 +++++++++--------- 10 files changed, 518 insertions(+), 203 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/L High Tide.path create mode 100644 src/main/deploy/pathplanner/paths/R High Tide.path diff --git a/src/main/deploy/pathplanner/paths/Depot - Middle.path b/src/main/deploy/pathplanner/paths/Depot - Middle.path index 7671e3b431..6879a67dbc 100644 --- a/src/main/deploy/pathplanner/paths/Depot - Middle.path +++ b/src/main/deploy/pathplanner/paths/Depot - Middle.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.698, - "y": 5.973 + "x": 3.2368616262482157, + "y": 5.412967189728959 }, "prevControl": null, "nextControl": { - "x": 1.698514684606482, - "y": 5.874260054976852 + "x": 4.19432239657632, + "y": 5.529415121255349 }, "isLocked": false, "linkedName": null @@ -20,12 +20,12 @@ "y": 5.555292439367715 }, "prevControl": { - "x": 4.955057996156298, - "y": 5.1432779506218695 + "x": 4.750684736091298, + "y": 5.438844507845934 }, "nextControl": { - "x": 7.07964336661683, - "y": 5.956390870180839 + "x": 7.149166461811526, + "y": 5.656888301084985 }, "isLocked": false, "linkedName": null @@ -36,8 +36,8 @@ "y": 5.801126961483595 }, "prevControl": { - "x": 7.274499942827901, - "y": 5.89397463057212 + "x": 7.274499942827899, + "y": 5.893974630572121 }, "nextControl": { "x": 7.8947788873038505, @@ -48,12 +48,12 @@ }, { "anchor": { - "x": 8.153552068471324, - "y": 4.442567760337759 + "x": 8.011226818830243, + "y": 4.766034236804566 }, "prevControl": { - "x": 8.05004279600342, - "y": 5.1024393723206405 + "x": 7.90771754636234, + "y": 5.425905848787447 }, "nextControl": null, "isLocked": false, @@ -61,10 +61,6 @@ } ], "rotationTargets": [ - { - "waypointRelativePos": 0.48312611012434026, - "rotationDegrees": -150.0 - }, { "waypointRelativePos": 0.976909413854349, "rotationDegrees": -150.0 @@ -111,7 +107,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": -146.82148834088233 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Depot - Starting line.path b/src/main/deploy/pathplanner/paths/Depot - Starting line.path index bb7c5106f3..803fd5289a 100644 --- a/src/main/deploy/pathplanner/paths/Depot - Starting line.path +++ b/src/main/deploy/pathplanner/paths/Depot - Starting line.path @@ -56,13 +56,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -34.70155660178564 + "rotation": 90.0 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": -51.766174822553026 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/L High Tide.path b/src/main/deploy/pathplanner/paths/L High Tide.path new file mode 100644 index 0000000000..0c67910ced --- /dev/null +++ b/src/main/deploy/pathplanner/paths/L High Tide.path @@ -0,0 +1,161 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.3533095577746077, + "y": 5.425905848787448 + }, + "prevControl": null, + "nextControl": { + "x": 4.520806795450207, + "y": 5.411193113701927 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.798716119828815, + "y": 5.425905848787448 + }, + "prevControl": { + "x": 5.548727895536896, + "y": 5.423479387295311 + }, + "nextControl": { + "x": 6.903001943813859, + "y": 5.436624381773559 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.27038794626047, + "y": 4.664813513445186 + }, + "prevControl": { + "x": 8.701588101689149, + "y": 7.251925360442476 + }, + "nextControl": { + "x": 8.201584412594418, + "y": 4.252006526198717 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.915164051355206, + "y": 4.3496718972895865 + }, + "prevControl": { + "x": 6.491196925034901, + "y": 4.261521174842816 + }, + "nextControl": { + "x": 5.432401753901503, + "y": 4.4235493567803275 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.56, + "y": 5.975825159143519 + }, + "prevControl": { + "x": 13.083181169757488, + "y": 5.076562054208274 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Depot Feed" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.0, + "rotationDegrees": -135.0 + }, + { + "waypointRelativePos": 1.3571134868421066, + "rotationDegrees": -180.0 + }, + { + "waypointRelativePos": 1.7769325657894743, + "rotationDegrees": 96.86659894451316 + }, + { + "waypointRelativePos": 1.9296375266524526, + "rotationDegrees": 95.12579413680578 + }, + { + "waypointRelativePos": 2.3, + "rotationDegrees": 0.8423256270445401 + }, + { + "waypointRelativePos": 2.7, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 2.95, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 3.1684434968017046, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 3.2324093816631017, + "rotationDegrees": 117.96194664745079 + }, + { + "waypointRelativePos": 3.2857142857142954, + "rotationDegrees": -141.06653828726158 + }, + { + "waypointRelativePos": 3.6908315565031975, + "rotationDegrees": -41.74490545615026 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 3.9020931802835923, + "maxWaypointRelativePos": 4.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/deploy/pathplanner/paths/L horseshoe.path b/src/main/deploy/pathplanner/paths/L horseshoe.path index 0eda820523..63470a8722 100644 --- a/src/main/deploy/pathplanner/paths/L horseshoe.path +++ b/src/main/deploy/pathplanner/paths/L horseshoe.path @@ -112,12 +112,12 @@ }, { "anchor": { - "x": 1.904179743223965, - "y": 2.500000000000001 + "x": 3.210984308131241, + "y": 2.4758915834522113 }, "prevControl": { - "x": 2.4666588375526692, - "y": 3.0030359519675933 + "x": 3.773463402459945, + "y": 2.9789275354198037 }, "nextControl": null, "isLocked": false, @@ -177,7 +177,7 @@ }, "goalEndState": { "velocity": 0.0, - "rotation": 80.579832280714 + "rotation": -142.25319461272534 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/Outpost - Middle.path b/src/main/deploy/pathplanner/paths/Outpost - Middle.path index e51a1422c8..b3d825bb87 100644 --- a/src/main/deploy/pathplanner/paths/Outpost - Middle.path +++ b/src/main/deploy/pathplanner/paths/Outpost - Middle.path @@ -3,41 +3,41 @@ "waypoints": [ { "anchor": { - "x": 1.904179743223965, - "y": 2.500000000000001 + "x": 3.210984308131241, + "y": 2.4758915834522113 }, "prevControl": null, "nextControl": { - "x": 3.2498002853067045, - "y": 2.3076890156918695 + "x": 4.556604850213983, + "y": 2.28358059914408 }, "isLocked": false, "linkedName": "Outpost alliance wait" }, { "anchor": { - "x": 6.976134094151213, - "y": 2.3076890156918695 + "x": 7.053766048502139, + "y": 2.4758915834522113 }, "prevControl": { - "x": 6.113849913328376, - "y": 1.9843324478833049 + "x": 6.15927901684682, + "y": 2.2568335348835613 }, "nextControl": { - "x": 7.597189728958634, - "y": 2.540584878744652 + "x": 7.687760342368046, + "y": 2.6311554921540665 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 8.050042796005705, - "y": 3.7180028530670475 + "x": 8.062981455064193, + "y": 3.291027104136948 }, "prevControl": { - "x": 7.90771754636234, - "y": 2.579400855920116 + "x": 7.95947218259629, + "y": 2.644094151212554 }, "nextControl": null, "isLocked": false, @@ -45,10 +45,6 @@ } ], "rotationTargets": [ - { - "waypointRelativePos": 0.26652452025586415, - "rotationDegrees": -150.0 - }, { "waypointRelativePos": 0.707889125799577, "rotationDegrees": -150.0 @@ -87,7 +83,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 80.579832280714 + "rotation": -142.25319461272534 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/R High Tide.path b/src/main/deploy/pathplanner/paths/R High Tide.path new file mode 100644 index 0000000000..043f09142d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/R High Tide.path @@ -0,0 +1,177 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.395203052662038, + "y": 2.5 + }, + "prevControl": null, + "nextControl": { + "x": 4.562700290337638, + "y": 2.5147127350855207 + }, + "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.575035663338087, + "y": 3.8603281027104144 + }, + "prevControl": { + "x": 8.20530670470756, + "y": 3.8344507845934386 + }, + "nextControl": { + "x": 6.15907468771046, + "y": 3.8669306578791067 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.915164051355206, + "y": 3.8603281027104144 + }, + "prevControl": { + "x": 6.491196925034901, + "y": 3.948478825157185 + }, + "nextControl": { + "x": 5.432401753901503, + "y": 3.7864506432196734 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.210984308131241, + "y": 2.4758915834522113 + }, + "prevControl": { + "x": 13.678359486447931, + "y": 2.4259058487874463 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Outpost alliance wait" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.0, + "rotationDegrees": 135.0 + }, + { + "waypointRelativePos": 1.3571134868421066, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 1.7769325657894743, + "rotationDegrees": -96.86659894451316 + }, + { + "waypointRelativePos": 1.9296375266524526, + "rotationDegrees": -95.12579413680578 + }, + { + "waypointRelativePos": 2.580468749999996, + "rotationDegrees": -0.8423256270445401 + }, + { + "waypointRelativePos": 3.4298930921052637, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 3.89, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 4.168443496801705, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 4.232409381663102, + "rotationDegrees": -117.96194664745079 + }, + { + "waypointRelativePos": 4.285714285714295, + "rotationDegrees": 141.06653828726158 + }, + { + "waypointRelativePos": 4.6908315565031975, + "rotationDegrees": 41.74490545615026 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 4.929101958136382, + "maxWaypointRelativePos": 5.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": -142.25319461272534 + }, + "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 light.path b/src/main/deploy/pathplanner/paths/R quarter light.path index 724185106f..a4257bf09e 100644 --- a/src/main/deploy/pathplanner/paths/R quarter light.path +++ b/src/main/deploy/pathplanner/paths/R quarter light.path @@ -80,12 +80,12 @@ }, { "anchor": { - "x": 1.904179743223965, - "y": 2.500000000000001 + "x": 3.210984308131241, + "y": 2.4758915834522113 }, "prevControl": { - "x": 2.1541797345840688, - "y": 2.4999342736871943 + "x": 3.4609842994913445, + "y": 2.4758258571394047 }, "nextControl": null, "isLocked": false, @@ -154,7 +154,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 80.579832280714 + "rotation": -142.25319461272534 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/R quarter.path b/src/main/deploy/pathplanner/paths/R quarter.path index ad2d863932..2077ad4418 100644 --- a/src/main/deploy/pathplanner/paths/R quarter.path +++ b/src/main/deploy/pathplanner/paths/R quarter.path @@ -80,12 +80,12 @@ }, { "anchor": { - "x": 1.904179743223965, - "y": 2.500000000000001 + "x": 3.210984308131241, + "y": 2.4758915834522113 }, "prevControl": { - "x": 2.4938613897909025, - "y": 2.500000000000001 + "x": 3.8006659546981783, + "y": 2.4758915834522113 }, "nextControl": null, "isLocked": false, @@ -156,7 +156,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 80.579832280714 + "rotation": -142.25319461272534 }, "reversed": false, "folder": null, diff --git a/src/main/java/frc/robot/autonomous/AutonomousConstants.java b/src/main/java/frc/robot/autonomous/AutonomousConstants.java index 07a0d60152..7737039942 100644 --- a/src/main/java/frc/robot/autonomous/AutonomousConstants.java +++ b/src/main/java/frc/robot/autonomous/AutonomousConstants.java @@ -35,6 +35,7 @@ public class AutonomousConstants { public static final double STEAL_START_SECOND_SINCE_AUTO_BEGAN = 3.5; public static final double SIDE_STEAL_START_SECOND_SINCE_AUTO_BEGAN = 3.5; public static final double TIME_BETWEEN_BALLS_TO_RETURN_TO_MIDDLE_SECONDS = 1.0; + public static final double TIME_BETWEEN_BALLS_TO_CLOSE_INTAKE_SECONDS = 1.0; public static final double TIME_BEFORE_AUTO_END_TO_RETURN_TO_MIDDLE_SECONDS_ON_OUTPOST_START = 4.0; public static final double TIME_BEFORE_AUTO_END_TO_RETURN_TO_MIDDLE_SECONDS_ON_DEPOT_START = 6.0; public static final double MINIMUM_TIME_AFTER_STARTING_TO_SHOOT_TO_RETURN_TO_MIDDLE = 1.0; diff --git a/src/main/java/frc/robot/autonomous/AutosBuilder.java b/src/main/java/frc/robot/autonomous/AutosBuilder.java index 06192ecfb3..2e77addeb0 100644 --- a/src/main/java/frc/robot/autonomous/AutosBuilder.java +++ b/src/main/java/frc/robot/autonomous/AutosBuilder.java @@ -110,18 +110,6 @@ public static List> getAutoList( true, returnToMiddle ), - getExtendedLQuarterAuto( - robot, - resetSubsystems, - openIntake, - closeIntake, - scoreSequence, - pathfindingConstraints, - regularIsNearEndOfPathTolerance, - stuckIsNearEndOfPathTolerance, - stuckDebounceSeconds, - returnToMiddle - ), getHorseshoeAuto( robot, resetSubsystems, @@ -503,20 +491,78 @@ private static Supplier getQuarterAuto( openIntake.get() .until(() -> hasPathEnded) .andThen( - new ParallelCommandGroup( - getAllianceSideToStartingLineAuto( - robot, - startingSide, - pathfindingConstraints, - regularIsNearEndOfPathTolerance, - stuckIsNearEndOfPathTolerance, - stuckDebounceSeconds, - returnToMiddle, - scoreSequence, - closeIntake, - true - ) - ) + getAllianceSideToStartingLineAuto( + robot, + startingSide, + pathfindingConstraints, + regularIsNearEndOfPathTolerance, + stuckIsNearEndOfPathTolerance, + stuckDebounceSeconds, + returnToMiddle, + scoreSequence, + closeIntake + ).asProxy() + ) + ) + ) + ), + new Pose2d(), + startingSide == AllianceSide.DEPOT ? "L quarter" : "R quarter", + startingSide == AllianceSide.DEPOT ? PathHelper.PATH_PLANNER_PATHS.get("L quarter") : PathHelper.PATH_PLANNER_PATHS.get("R quarter"), + getAllianceSideToStartingLinePath(startingSide), + getAllianceSideToMiddlePath(startingSide) + ); + } + + private static Supplier getHighTideAuto( + Robot robot, + Supplier resetSubsystems, + Supplier openIntake, + Supplier closeIntake, + Supplier scoreSequence, + PathConstraints pathfindingConstraints, + Pose2d regularIsNearEndOfPathTolerance, + Pose2d stuckIsNearEndOfPathTolerance, + double stuckDebounceSeconds, + AllianceSide startingSide, + BooleanSupplier returnToMiddle + ) { + return () -> new PathPlannerAutoWrapper( + new ParallelCommandGroup( + PathFollowingCommandsBuilder + .followAdjustedPathThenStop( + robot.getSwerve(), + () -> robot.getPoseEstimator().getEstimatedPose(), + startingSide == AllianceSide.DEPOT + ? PathHelper.PATH_PLANNER_PATHS.get("L High Tide") + : PathHelper.PATH_PLANNER_PATHS.get("R HighTide"), + 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( + getAllianceSideToStartingLineAuto( + robot, + startingSide, + pathfindingConstraints, + regularIsNearEndOfPathTolerance, + stuckIsNearEndOfPathTolerance, + stuckDebounceSeconds, + returnToMiddle, + scoreSequence, + closeIntake + ).asProxy() ) ) ) @@ -524,10 +570,12 @@ private static Supplier getQuarterAuto( new Pose2d(), startingSide == AllianceSide.DEPOT ? "L quarter" : "R quarter", startingSide == AllianceSide.DEPOT ? PathHelper.PATH_PLANNER_PATHS.get("L quarter") : PathHelper.PATH_PLANNER_PATHS.get("R quarter"), - getAllianceSideToStartingLinePath(startingSide, returnToMiddle, true) + getAllianceSideToStartingLinePath(startingSide), + getAllianceSideToMiddlePath(startingSide) ); } + private static Supplier getSideAuto( Robot robot, Supplier resetSubsystems, @@ -606,14 +654,12 @@ private static Supplier getSideAuto( stuckDebounceSeconds, returnToMiddle, scoreSequence, - closeIntake, - false + closeIntake ) ) .asProxy(), new WaitCommand(AutonomousConstants.TIME_TO_WAIT_TO_CLOSE_INTAKE_AFTER_PATH_END_SECONDS) .andThen(closeIntake.get()) - .onlyIf(() -> !returnToMiddle.getAsBoolean()) ) ) ) @@ -722,14 +768,12 @@ private static Supplier getStealAuto( stuckDebounceSeconds, returnToMiddle, scoreSequence, - closeIntake, - false + closeIntake ) ) .asProxy(), new WaitCommand(AutonomousConstants.TIME_TO_WAIT_TO_CLOSE_INTAKE_AFTER_PATH_END_SECONDS) .andThen(closeIntake.get()) - .onlyIf(() -> !returnToMiddle.getAsBoolean()) ) ) ) @@ -741,7 +785,8 @@ private static Supplier getStealAuto( ? PathHelper.PATH_PLANNER_PATHS.get("Depot Hub Wait") : PathHelper.PATH_PLANNER_PATHS.get("Outpost Hub Wait"), getStealPath(firstOpponentBumpSide, returnSide, skipOutpost), - getAllianceSideToStartingLinePath(actualReturnSide, returnToMiddle, false) + getAllianceSideToStartingLinePath(startingSide), + getAllianceSideToMiddlePath(startingSide) ); } @@ -821,14 +866,12 @@ private static Supplier getLightStealAuto( stuckDebounceSeconds, returnToMiddle, scoreSequence, - closeIntake, - false + closeIntake ) ) .asProxy(), new WaitCommand(AutonomousConstants.TIME_TO_WAIT_TO_CLOSE_INTAKE_AFTER_PATH_END_SECONDS) .andThen(closeIntake.get()) - .onlyIf(() -> !returnToMiddle.getAsBoolean()) ) ) ) @@ -886,8 +929,6 @@ private static Supplier getLightQuarterAuto( openIntake.get() .until(() -> hasPathEnded) .andThen( - new ParallelCommandGroup( - getAllianceSideToStartingLineAuto( robot, startingSide, @@ -897,13 +938,11 @@ private static Supplier getLightQuarterAuto( stuckDebounceSeconds, returnToMiddle, scoreSequence, - closeIntake, - true + closeIntake ).asProxy(), new WaitCommand(AutonomousConstants.TIME_TO_WAIT_TO_CLOSE_INTAKE_AFTER_PATH_END_SECONDS) .andThen(closeIntake.get()) - .onlyIf(() -> !returnToMiddle.getAsBoolean()) - ) + ) ) ) @@ -915,80 +954,8 @@ private static Supplier getLightQuarterAuto( ? PathHelper.PATH_PLANNER_PATHS.get("L quarter light to outpost") : PathHelper.PATH_PLANNER_PATHS.get("L quarter light") : PathHelper.PATH_PLANNER_PATHS.get("R quarter light"), - getAllianceSideToStartingLinePath(startingSide, returnToMiddle, true) - ); - } - - private static Supplier getExtendedLQuarterAuto( - Robot robot, - Supplier resetSubsystems, - Supplier openIntake, - Supplier closeIntake, - Supplier scoreSequence, - PathConstraints pathfindingConstraints, - Pose2d regularIsNearEndOfPathTolerance, - Pose2d stuckIsNearEndOfPathTolerance, - double stuckDebounceSeconds, - BooleanSupplier returnToMiddle - ) { - return () -> new PathPlannerAutoWrapper( - new ParallelCommandGroup( - PathFollowingCommandsBuilder - .followAdjustedPathThenStop( - robot.getSwerve(), - () -> robot.getPoseEstimator().getEstimatedPose(), - PathHelper.PATH_PLANNER_PATHS.get("L quarter to outpost"), - 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) - .withDeadline(new WaitCommand(AutonomousConstants.TIME_TO_WAIT_AT_DEPOT)) - ) - .andThen( - getAllianceSideToStartingLineAuto( - robot, - AllianceSide.OUTPOST, - pathfindingConstraints, - regularIsNearEndOfPathTolerance, - stuckIsNearEndOfPathTolerance, - stuckDebounceSeconds, - returnToMiddle, - scoreSequence, - closeIntake, - false - ) - ) - .asProxy(), - new WaitCommand(AutonomousConstants.TIME_TO_WAIT_TO_CLOSE_INTAKE_AFTER_PATH_END_SECONDS) - .andThen(closeIntake.get()) - .onlyIf(() -> !returnToMiddle.getAsBoolean()) - ) - ) - ) - ) - ), - new Pose2d(), - "L quarter to outpost", - PathHelper.PATH_PLANNER_PATHS.get("L quarter to outpost"), - PathHelper.PATH_PLANNER_PATHS.get("Outpost - Starting line") + getAllianceSideToStartingLinePath(startingSide), + getAllianceSideToMiddlePath(startingSide) ); } @@ -1048,14 +1015,12 @@ private static Supplier getHorseshoeAuto( stuckDebounceSeconds, returnToMiddle, scoreSequence, - closeIntake, - false + closeIntake ) ) .asProxy(), new WaitCommand(AutonomousConstants.TIME_TO_WAIT_TO_CLOSE_INTAKE_AFTER_PATH_END_SECONDS) .andThen(closeIntake.get()) - .onlyIf(() -> !returnToMiddle.getAsBoolean()) ) ) ) @@ -1066,7 +1031,8 @@ private static Supplier getHorseshoeAuto( startingSide == AllianceSide.DEPOT ? PathHelper.PATH_PLANNER_PATHS.get("L horseshoe") : PathHelper.PATH_PLANNER_PATHS.get("R horseshoe"), - getAllianceSideToStartingLinePath(startingSide, returnToMiddle, false) + getAllianceSideToStartingLinePath(startingSide), + getAllianceSideToMiddlePath(startingSide) ); } @@ -1191,22 +1157,26 @@ private static Command getAllianceSideToStartingLineAuto( double stuckDebounceSeconds, BooleanSupplier returnToMiddle, Supplier scoreSequence, - Supplier closeIntake, - boolean isQuarter + Supplier closeIntake ) { return new ParallelCommandGroup( new WaitCommand(AutonomousConstants.MINIMUM_TIME_AFTER_STARTING_TO_SHOOT_TO_RETURN_TO_MIDDLE) - .andThen(new RunCommand(() -> {}).until(() -> hasStoppedThrowingBalls(robot))) - .andThen(closeIntake.get()), + .andThen(new RunCommand(() -> {}).until(() -> isReadyToCloseIntake(robot))) + .andThen( + new ParallelCommandGroup(closeIntake.get()), + PathFollowingCommandsBuilder.followAdjustedPathThenStop( + robot.getSwerve(), + () -> robot.getPoseEstimator().getEstimatedPose(), + getAllianceSideToStartingLinePath(allianceSide), + pathfindingConstraints, + regularIsNearEndOfPathTolerance, + stuckIsNearEndOfPathTolerance, + stuckDebounceSeconds, + robot.getSwerve().getLogPath() + ) + ), scoreSequence.get() - ).until( - () -> returnToMiddle.getAsBoolean() - && TimeUtil.getCurrentTimeSeconds() - TimeUtil.getAutonomousStartTimeSeconds() - > GamePeriodUtils.AUTONOMOUS_DURATION_SECONDS - - (allianceSide == AllianceSide.OUTPOST - ? AutonomousConstants.TIME_BEFORE_AUTO_END_TO_RETURN_TO_MIDDLE_SECONDS_ON_OUTPOST_START - : AutonomousConstants.TIME_BEFORE_AUTO_END_TO_RETURN_TO_MIDDLE_SECONDS_ON_DEPOT_START) - ) + ).until(isReadyToReturnToMiddle(robot, returnToMiddle, allianceSide)) .andThen( new ParallelCommandGroup( scoreSequence.get(), @@ -1215,7 +1185,7 @@ private static Command getAllianceSideToStartingLineAuto( .followAdjustedPathThenStop( robot.getSwerve(), () -> robot.getPoseEstimator().getEstimatedPose(), - getAllianceSideToStartingLinePath(allianceSide, returnToMiddle, isQuarter), + getAllianceSideToMiddlePath(allianceSide), pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -1232,19 +1202,14 @@ private static Command getAllianceSideToStartingLineAuto( ); } - private static PathPlannerPath getAllianceSideToStartingLinePath( - AllianceSide allianceSide, - BooleanSupplier returnToMiddle, - boolean isQuarter - ) { - if (returnToMiddle.getAsBoolean()) { - return allianceSide == AllianceSide.DEPOT - ? PathHelper.PATH_PLANNER_PATHS.get("Depot - Middle") - : PathHelper.PATH_PLANNER_PATHS.get("Outpost - Middle"); - } - if (isQuarter && allianceSide == AllianceSide.OUTPOST) { - return PathHelper.PATH_PLANNER_PATHS.get("R alliance stay"); - } + private static PathPlannerPath getAllianceSideToMiddlePath(AllianceSide allianceSide) { + return allianceSide == AllianceSide.DEPOT + ? PathHelper.PATH_PLANNER_PATHS.get("Depot - Middle") + : PathHelper.PATH_PLANNER_PATHS.get("Outpost - Middle"); + } + + + private static PathPlannerPath getAllianceSideToStartingLinePath(AllianceSide allianceSide) { return allianceSide == AllianceSide.DEPOT ? PathHelper.PATH_PLANNER_PATHS.get("Depot - Starting line") : PathHelper.PATH_PLANNER_PATHS.get("Outpost - Starting line"); @@ -1252,11 +1217,30 @@ private static PathPlannerPath getAllianceSideToStartingLinePath( private static boolean hasStoppedThrowingBalls(Robot robot) { + return isTimeBetweenBallsAboveThreshold(robot, AutonomousConstants.TIME_BETWEEN_BALLS_TO_RETURN_TO_MIDDLE_SECONDS); + } + + private static boolean isReadyToCloseIntake(Robot robot) { + return isTimeBetweenBallsAboveThreshold(robot, AutonomousConstants.TIME_BETWEEN_BALLS_TO_CLOSE_INTAKE_SECONDS); + } + + private static boolean isTimeBetweenBallsAboveThreshold(Robot robot, double threshold) { double lastBallTimestamp = robot.getBallsBufferIncludingPassing().getInternalBuffer().lastEntry() != null ? robot.getBallsBufferIncludingPassing().getInternalBuffer().lastEntry().getKey() : 0; - return TimeUtil.getCurrentTimeSeconds() - lastBallTimestamp > AutonomousConstants.TIME_BETWEEN_BALLS_TO_RETURN_TO_MIDDLE_SECONDS; + return TimeUtil.getCurrentTimeSeconds() - lastBallTimestamp > threshold; + } + + private static BooleanSupplier isReadyToReturnToMiddle(Robot robot, BooleanSupplier returnToMiddleByTimer, AllianceSide allianceSide) { + return () -> hasStoppedThrowingBalls(robot) + || (returnToMiddleByTimer.getAsBoolean() + && TimeUtil.getCurrentTimeSeconds() - TimeUtil.getAutonomousStartTimeSeconds() + > GamePeriodUtils.AUTONOMOUS_DURATION_SECONDS + - (allianceSide == AllianceSide.OUTPOST + ? AutonomousConstants.TIME_BEFORE_AUTO_END_TO_RETURN_TO_MIDDLE_SECONDS_ON_OUTPOST_START + : AutonomousConstants.TIME_BEFORE_AUTO_END_TO_RETURN_TO_MIDDLE_SECONDS_ON_DEPOT_START)); } + } From 3d18f6ae36d8b99267f3dfbf692617dfad433f80 Mon Sep 17 00:00:00 2001 From: itamaroryan Date: Fri, 3 Jul 2026 18:23:30 +0300 Subject: [PATCH 2/8] fixes --- .../robot/autonomous/AutonomousConstants.java | 5 +- .../frc/robot/autonomous/AutosBuilder.java | 131 ++++++++++-------- 2 files changed, 80 insertions(+), 56 deletions(-) diff --git a/src/main/java/frc/robot/autonomous/AutonomousConstants.java b/src/main/java/frc/robot/autonomous/AutonomousConstants.java index 7737039942..9b10939c74 100644 --- a/src/main/java/frc/robot/autonomous/AutonomousConstants.java +++ b/src/main/java/frc/robot/autonomous/AutonomousConstants.java @@ -34,11 +34,12 @@ public class AutonomousConstants { public static final double TIME_FOR_SECOND_OUTTAKE_IN_PUSH_AUTO = 1.75; public static final double STEAL_START_SECOND_SINCE_AUTO_BEGAN = 3.5; public static final double SIDE_STEAL_START_SECOND_SINCE_AUTO_BEGAN = 3.5; - public static final double TIME_BETWEEN_BALLS_TO_RETURN_TO_MIDDLE_SECONDS = 1.0; - public static final double TIME_BETWEEN_BALLS_TO_CLOSE_INTAKE_SECONDS = 1.0; + public static final double TIME_BETWEEN_BALLS_TO_RETURN_TO_MIDDLE_SECONDS = 0.6; + public static final double TIME_BETWEEN_BALLS_TO_CLOSE_INTAKE_SECONDS = 0.3; public static final double TIME_BEFORE_AUTO_END_TO_RETURN_TO_MIDDLE_SECONDS_ON_OUTPOST_START = 4.0; public static final double TIME_BEFORE_AUTO_END_TO_RETURN_TO_MIDDLE_SECONDS_ON_DEPOT_START = 6.0; public static final double MINIMUM_TIME_AFTER_STARTING_TO_SHOOT_TO_RETURN_TO_MIDDLE = 1.0; + public static final double MINIMUM_TIME_AFTER_CLOSING_INTAKE_TO_RETURN_TO_MIDDLE = 0.3; 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 80ba4c1432..d404b0d27b 100644 --- a/src/main/java/frc/robot/autonomous/AutosBuilder.java +++ b/src/main/java/frc/robot/autonomous/AutosBuilder.java @@ -449,6 +449,32 @@ public static List> getAutoList( AllianceSide.OUTPOST, true, returnToMiddle + ), + getHighTideAuto( + robot, + resetSubsystems, + openIntake, + closeIntake, + scoreSequence, + pathfindingConstraints, + regularIsNearEndOfPathTolerance, + stuckIsNearEndOfPathTolerance, + stuckDebounceSeconds, + AllianceSide.OUTPOST, + returnToMiddle + ), + getHighTideAuto( + robot, + resetSubsystems, + openIntake, + closeIntake, + scoreSequence, + pathfindingConstraints, + regularIsNearEndOfPathTolerance, + stuckIsNearEndOfPathTolerance, + stuckDebounceSeconds, + AllianceSide.DEPOT, + returnToMiddle ) ); } @@ -489,22 +515,22 @@ private static Supplier getQuarterAuto( new ParallelCommandGroup( new WaitCommand(AutonomousConstants.TIME_TO_WAIT_TO_START_SHOOTING_AFTER_AUTO_START).andThen(scoreSequence.get()), openIntake.get() - .until(() -> hasPathEnded) - .andThen( - getAllianceSideToStartingLineAuto( - robot, - startingSide, - pathfindingConstraints, - regularIsNearEndOfPathTolerance, - stuckIsNearEndOfPathTolerance, - stuckDebounceSeconds, - returnToMiddle, - scoreSequence, - closeIntake, - openIntake - ).asProxy() - ) - ) + ).until(() -> hasPathEnded) + .andThen( + getAllianceSideToStartingLineAuto( + robot, + startingSide, + pathfindingConstraints, + regularIsNearEndOfPathTolerance, + stuckIsNearEndOfPathTolerance, + stuckDebounceSeconds, + returnToMiddle, + scoreSequence, + closeIntake, + openIntake + ).asProxy() + + ) ) ), new Pose2d(), @@ -536,7 +562,7 @@ private static Supplier getHighTideAuto( () -> robot.getPoseEstimator().getEstimatedPose(), startingSide == AllianceSide.DEPOT ? PathHelper.PATH_PLANNER_PATHS.get("L High Tide") - : PathHelper.PATH_PLANNER_PATHS.get("R HighTide"), + : PathHelper.PATH_PLANNER_PATHS.get("R High Tide"), pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -570,8 +596,10 @@ private static Supplier getHighTideAuto( ) ), new Pose2d(), - startingSide == AllianceSide.DEPOT ? "L quarter" : "R quarter", - startingSide == AllianceSide.DEPOT ? PathHelper.PATH_PLANNER_PATHS.get("L quarter") : PathHelper.PATH_PLANNER_PATHS.get("R quarter"), + startingSide == AllianceSide.DEPOT ? "L High Tide" : "R High Tide", + startingSide == AllianceSide.DEPOT + ? PathHelper.PATH_PLANNER_PATHS.get("L High Tide") + : PathHelper.PATH_PLANNER_PATHS.get("R High Tide"), getAllianceSideToStartingLinePath(startingSide), getAllianceSideToMiddlePath(startingSide) ); @@ -1168,45 +1196,41 @@ private static Command getAllianceSideToStartingLineAuto( Supplier closeIntake, Supplier openIntake ) { - return new ParallelCommandGroup( + return new ParallelDeadlineGroup( new WaitCommand(AutonomousConstants.MINIMUM_TIME_AFTER_STARTING_TO_SHOOT_TO_RETURN_TO_MIDDLE) .andThen(new RunCommand(() -> {}).until(() -> isReadyToCloseIntake(robot))) .andThen( - new ParallelCommandGroup(closeIntake.get()), + new ParallelDeadlineGroup( + new WaitCommand(AutonomousConstants.MINIMUM_TIME_AFTER_CLOSING_INTAKE_TO_RETURN_TO_MIDDLE) + .andThen(new RunCommand(() -> {}).until(() -> hasStoppedThrowingBalls(robot))), + closeIntake.get(), + PathFollowingCommandsBuilder.followAdjustedPathThenStop( + robot.getSwerve(), + () -> robot.getPoseEstimator().getEstimatedPose(), + getAllianceSideToStartingLinePath(allianceSide), + pathfindingConstraints, + regularIsNearEndOfPathTolerance, + stuckIsNearEndOfPathTolerance, + stuckDebounceSeconds, + robot.getSwerve().getLogPath() + ) + ) + ), + scoreSequence.get() + ).until(isReadyToReturnToMiddle(returnToMiddle, allianceSide)) + .andThen( + new ParallelCommandGroup( + openIntake.get(), PathFollowingCommandsBuilder.followAdjustedPathThenStop( robot.getSwerve(), () -> robot.getPoseEstimator().getEstimatedPose(), - getAllianceSideToStartingLinePath(allianceSide), + getAllianceSideToMiddlePath(allianceSide), pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, stuckDebounceSeconds, robot.getSwerve().getLogPath() ) - ), - scoreSequence.get() - ).until(isReadyToReturnToMiddle(robot, returnToMiddle, allianceSide)) - .andThen( - new ParallelCommandGroup( - scoreSequence.get(), - openIntake.get(), - PathFollowingCommandsBuilder - .followAdjustedPathThenStop( - robot.getSwerve(), - () -> robot.getPoseEstimator().getEstimatedPose(), - getAllianceSideToMiddlePath(allianceSide), - pathfindingConstraints, - regularIsNearEndOfPathTolerance, - stuckIsNearEndOfPathTolerance, - stuckDebounceSeconds, - robot.getSwerve().getLogPath() - ) - .andThen( - robot.getSwerve() - .getCommandsBuilder() - .wiggle(AutonomousConstants.WIGGLE_RANGE, AutonomousConstants.TIME_BETWEEN_WIGGLES_SECONDS) - .onlyIf(() -> !returnToMiddle.getAsBoolean()) - ) ) ); } @@ -1241,14 +1265,13 @@ private static boolean isTimeBetweenBallsAboveThreshold(Robot robot, double thre return TimeUtil.getCurrentTimeSeconds() - lastBallTimestamp > threshold; } - private static BooleanSupplier isReadyToReturnToMiddle(Robot robot, BooleanSupplier returnToMiddleByTimer, AllianceSide allianceSide) { - return () -> hasStoppedThrowingBalls(robot) - || (returnToMiddleByTimer.getAsBoolean() - && TimeUtil.getCurrentTimeSeconds() - TimeUtil.getAutonomousStartTimeSeconds() - > GamePeriodUtils.AUTONOMOUS_DURATION_SECONDS - - (allianceSide == AllianceSide.OUTPOST - ? AutonomousConstants.TIME_BEFORE_AUTO_END_TO_RETURN_TO_MIDDLE_SECONDS_ON_OUTPOST_START - : AutonomousConstants.TIME_BEFORE_AUTO_END_TO_RETURN_TO_MIDDLE_SECONDS_ON_DEPOT_START)); + private static BooleanSupplier isReadyToReturnToMiddle(BooleanSupplier returnToMiddleByTimer, AllianceSide allianceSide) { + return () -> (returnToMiddleByTimer.getAsBoolean() + && TimeUtil.getCurrentTimeSeconds() - TimeUtil.getAutonomousStartTimeSeconds() + > GamePeriodUtils.AUTONOMOUS_DURATION_SECONDS + - (allianceSide == AllianceSide.OUTPOST + ? AutonomousConstants.TIME_BEFORE_AUTO_END_TO_RETURN_TO_MIDDLE_SECONDS_ON_OUTPOST_START + : AutonomousConstants.TIME_BEFORE_AUTO_END_TO_RETURN_TO_MIDDLE_SECONDS_ON_DEPOT_START)); } From 9f7910bc780525cd9703aa618c2548a75c035e72 Mon Sep 17 00:00:00 2001 From: YHerm <0549405422.yh@gmail.com> Date: Sat, 4 Jul 2026 18:16:28 +0300 Subject: [PATCH 3/8] better path --- .../paths/Depot - Starting line.path | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Depot - Starting line.path b/src/main/deploy/pathplanner/paths/Depot - Starting line.path index 803fd5289a..498a4c43e5 100644 --- a/src/main/deploy/pathplanner/paths/Depot - Starting line.path +++ b/src/main/deploy/pathplanner/paths/Depot - Starting line.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 3.11212048192771, - "y": 5.712403614457831 + "x": 3.1462910128388017, + "y": 5.555292439372327 }, "prevControl": { - "x": 1.7753156184874421, - "y": 5.508890246069346 + "x": 1.7701477063747146, + "y": 5.825157013216921 }, "nextControl": null, "isLocked": false, @@ -47,8 +47,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.2, - "maxAcceleration": 2.0, + "maxVelocity": 1.5, + "maxAcceleration": 1.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -56,13 +56,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 90.0 + "rotation": -99.12360199661283 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -51.766174822553026 + "rotation": 0.9279887127821316 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file From 00ceaae2b766d9512349beea32a12f388e183b27 Mon Sep 17 00:00:00 2001 From: YHerm <0549405422.yh@gmail.com> Date: Sat, 4 Jul 2026 19:12:04 +0300 Subject: [PATCH 4/8] paths --- src/main/deploy/pathplanner/paths/Depot - Middle.path | 10 +++++----- .../pathplanner/paths/Depot - Starting line.path | 10 +++++----- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Depot - Middle.path b/src/main/deploy/pathplanner/paths/Depot - Middle.path index 6879a67dbc..4161732151 100644 --- a/src/main/deploy/pathplanner/paths/Depot - Middle.path +++ b/src/main/deploy/pathplanner/paths/Depot - Middle.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 3.2368616262482157, - "y": 5.412967189728959 + "x": 3.4956348074179737, + "y": 5.555292439367715 }, "prevControl": null, "nextControl": { - "x": 4.19432239657632, - "y": 5.529415121255349 + "x": 4.453095577746078, + "y": 5.671740370894105 }, "isLocked": false, - "linkedName": null + "linkedName": "Depot" }, { "anchor": { diff --git a/src/main/deploy/pathplanner/paths/Depot - Starting line.path b/src/main/deploy/pathplanner/paths/Depot - Starting line.path index 498a4c43e5..35b65ef8d7 100644 --- a/src/main/deploy/pathplanner/paths/Depot - Starting line.path +++ b/src/main/deploy/pathplanner/paths/Depot - Starting line.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 3.1462910128388017, - "y": 5.555292439372327 + "x": 3.2498002853067045, + "y": 5.619985734664765 }, "prevControl": { - "x": 1.7701477063747146, - "y": 5.825157013216921 + "x": 1.8736569788426172, + "y": 5.889850308509359 }, "nextControl": null, "isLocked": false, @@ -56,7 +56,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -99.12360199661283 + "rotation": -139.1849161256499 }, "reversed": false, "folder": null, From 797831ddf4729f32810573d04f92d8f1588d4e0a Mon Sep 17 00:00:00 2001 From: greenblitz4590 <94316058+greenblitz4590@users.noreply.github.com> Date: Mon, 6 Jul 2026 00:13:14 +0300 Subject: [PATCH 5/8] code --- .../pathplanner/paths/Depot - Middle.path | 8 +++---- .../deploy/pathplanner/paths/L horseshoe.path | 10 ++++---- .../pathplanner/paths/Outpost - Middle.path | 12 +++++----- .../paths/Outpost - Starting line.path | 24 +++++++++---------- .../deploy/pathplanner/paths/R High Tide.path | 10 ++++---- .../pathplanner/paths/R quarter light.path | 10 ++++---- .../deploy/pathplanner/paths/R quarter.path | 10 ++++---- src/main/java/frc/robot/Robot.java | 3 +++ .../robot/autonomous/AutonomousConstants.java | 4 ++-- .../frc/robot/autonomous/AutosBuilder.java | 6 +++-- 10 files changed, 51 insertions(+), 46 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Depot - Middle.path b/src/main/deploy/pathplanner/paths/Depot - Middle.path index 4161732151..b24c6e901e 100644 --- a/src/main/deploy/pathplanner/paths/Depot - Middle.path +++ b/src/main/deploy/pathplanner/paths/Depot - Middle.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.4956348074179737, - "y": 5.555292439367715 + "x": 2.690439404677534, + "y": 5.375251594613749 }, "prevControl": null, "nextControl": { - "x": 4.453095577746078, - "y": 5.671740370894105 + "x": 3.6479001750056397, + "y": 5.491699526140139 }, "isLocked": false, "linkedName": "Depot" diff --git a/src/main/deploy/pathplanner/paths/L horseshoe.path b/src/main/deploy/pathplanner/paths/L horseshoe.path index 63470a8722..2245f78659 100644 --- a/src/main/deploy/pathplanner/paths/L horseshoe.path +++ b/src/main/deploy/pathplanner/paths/L horseshoe.path @@ -112,12 +112,12 @@ }, { "anchor": { - "x": 3.210984308131241, - "y": 2.4758915834522113 + "x": 2.6390148830616584, + "y": 2.3733451452870304 }, "prevControl": { - "x": 3.773463402459945, - "y": 2.9789275354198037 + "x": 3.2014939773903626, + "y": 2.876381097254623 }, "nextControl": null, "isLocked": false, @@ -177,7 +177,7 @@ }, "goalEndState": { "velocity": 0.0, - "rotation": -142.25319461272534 + "rotation": 130.9800700830008 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/Outpost - Middle.path b/src/main/deploy/pathplanner/paths/Outpost - Middle.path index b3d825bb87..8f31e499d0 100644 --- a/src/main/deploy/pathplanner/paths/Outpost - Middle.path +++ b/src/main/deploy/pathplanner/paths/Outpost - Middle.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 3.210984308131241, - "y": 2.4758915834522113 + "x": 2.908993621545004, + "y": 2.5790432317505316 }, "prevControl": null, "nextControl": { - "x": 4.556604850213983, - "y": 2.28358059914408 + "x": 4.254614163627744, + "y": 2.3867322474424 }, "isLocked": false, - "linkedName": "Outpost alliance wait" + "linkedName": "Outpost middle return" }, { "anchor": { @@ -83,7 +83,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -142.25319461272534 + "rotation": 131.63353933657012 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Outpost - Starting line.path b/src/main/deploy/pathplanner/paths/Outpost - Starting line.path index 7534df1f7e..950aa06741 100644 --- a/src/main/deploy/pathplanner/paths/Outpost - Starting line.path +++ b/src/main/deploy/pathplanner/paths/Outpost - Starting line.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 0.643, - "y": 0.736 + "x": 2.6390148830616584, + "y": 2.3733451452870304 }, "prevControl": null, "nextControl": { - "x": 1.410966233360837, - "y": 1.0471295424792166 + "x": 2.8530911402987873, + "y": 2.50246275093143 }, "isLocked": false, - "linkedName": null + "linkedName": "Outpost alliance wait" }, { "anchor": { - "x": 2.942740963855422, - "y": 2.341204819277109 + "x": 2.908993621545004, + "y": 2.5790432317505316 }, "prevControl": { - "x": -0.5548631101832293, - "y": -0.026744505700506238 + "x": 2.6390148830616584, + "y": 2.4183416017009214 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "Outpost middle return" } ], "rotationTargets": [], @@ -56,13 +56,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 135.0 + "rotation": 131.63353933657012 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": 130.9800700830008 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/R High Tide.path b/src/main/deploy/pathplanner/paths/R High Tide.path index 043f09142d..7e8351d0c0 100644 --- a/src/main/deploy/pathplanner/paths/R High Tide.path +++ b/src/main/deploy/pathplanner/paths/R High Tide.path @@ -80,12 +80,12 @@ }, { "anchor": { - "x": 3.210984308131241, - "y": 2.4758915834522113 + "x": 2.6390148830616584, + "y": 2.3733451452870304 }, "prevControl": { - "x": 13.678359486447931, - "y": 2.4259058487874463 + "x": 13.106390061378349, + "y": 2.3233594106222655 }, "nextControl": null, "isLocked": false, @@ -165,7 +165,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -142.25319461272534 + "rotation": 130.9800700830008 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/R quarter light.path b/src/main/deploy/pathplanner/paths/R quarter light.path index a4257bf09e..62fe720f3a 100644 --- a/src/main/deploy/pathplanner/paths/R quarter light.path +++ b/src/main/deploy/pathplanner/paths/R quarter light.path @@ -80,12 +80,12 @@ }, { "anchor": { - "x": 3.210984308131241, - "y": 2.4758915834522113 + "x": 2.6390148830616584, + "y": 2.3733451452870304 }, "prevControl": { - "x": 3.4609842994913445, - "y": 2.4758258571394047 + "x": 2.889014874421762, + "y": 2.373279418974224 }, "nextControl": null, "isLocked": false, @@ -154,7 +154,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -142.25319461272534 + "rotation": 130.9800700830008 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/R quarter.path b/src/main/deploy/pathplanner/paths/R quarter.path index 2077ad4418..d27c642a56 100644 --- a/src/main/deploy/pathplanner/paths/R quarter.path +++ b/src/main/deploy/pathplanner/paths/R quarter.path @@ -80,12 +80,12 @@ }, { "anchor": { - "x": 3.210984308131241, - "y": 2.4758915834522113 + "x": 2.6390148830616584, + "y": 2.3733451452870304 }, "prevControl": { - "x": 3.8006659546981783, - "y": 2.4758915834522113 + "x": 3.228696529628596, + "y": 2.3733451452870304 }, "nextControl": null, "isLocked": false, @@ -156,7 +156,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -142.25319461272534 + "rotation": 130.9800700830008 }, "reversed": false, "folder": null, diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a68886440a..504e0fcd9e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -341,6 +341,9 @@ public void periodic() { GamePeriodUtils.log(); + Logger.recordOutput("IsReadyToCloseIntakeForAutonomous", AutosBuilder.isReadyToCloseIntake(this)); + Logger.recordOutput("HasStoppedThrowingBallsForAutonomous", AutosBuilder.hasStoppedThrowingBalls(this)); + BatteryUtil.logStatus(); BusChain.logChainsStatuses(); CommandScheduler.getInstance().run(); // Should be last diff --git a/src/main/java/frc/robot/autonomous/AutonomousConstants.java b/src/main/java/frc/robot/autonomous/AutonomousConstants.java index 9b10939c74..9adfa62c20 100644 --- a/src/main/java/frc/robot/autonomous/AutonomousConstants.java +++ b/src/main/java/frc/robot/autonomous/AutonomousConstants.java @@ -34,8 +34,8 @@ public class AutonomousConstants { public static final double TIME_FOR_SECOND_OUTTAKE_IN_PUSH_AUTO = 1.75; public static final double STEAL_START_SECOND_SINCE_AUTO_BEGAN = 3.5; public static final double SIDE_STEAL_START_SECOND_SINCE_AUTO_BEGAN = 3.5; - public static final double TIME_BETWEEN_BALLS_TO_RETURN_TO_MIDDLE_SECONDS = 0.6; - public static final double TIME_BETWEEN_BALLS_TO_CLOSE_INTAKE_SECONDS = 0.3; + public static final double TIME_BETWEEN_BALLS_TO_RETURN_TO_MIDDLE_SECONDS = 1; + public static final double TIME_BETWEEN_BALLS_TO_CLOSE_INTAKE_SECONDS = 0.6; public static final double TIME_BEFORE_AUTO_END_TO_RETURN_TO_MIDDLE_SECONDS_ON_OUTPOST_START = 4.0; public static final double TIME_BEFORE_AUTO_END_TO_RETURN_TO_MIDDLE_SECONDS_ON_DEPOT_START = 6.0; public static final double MINIMUM_TIME_AFTER_STARTING_TO_SHOOT_TO_RETURN_TO_MIDDLE = 1.0; diff --git a/src/main/java/frc/robot/autonomous/AutosBuilder.java b/src/main/java/frc/robot/autonomous/AutosBuilder.java index d404b0d27b..fa44a425a0 100644 --- a/src/main/java/frc/robot/autonomous/AutosBuilder.java +++ b/src/main/java/frc/robot/autonomous/AutosBuilder.java @@ -10,6 +10,7 @@ import frc.utils.auto.PathHelper; import frc.utils.auto.PathPlannerAutoWrapper; import frc.utils.time.TimeUtil; +import org.littletonrobotics.junction.Logger; import java.util.List; import java.util.function.BooleanSupplier; @@ -1220,6 +1221,7 @@ private static Command getAllianceSideToStartingLineAuto( ).until(isReadyToReturnToMiddle(returnToMiddle, allianceSide)) .andThen( new ParallelCommandGroup( + scoreSequence.get(), openIntake.get(), PathFollowingCommandsBuilder.followAdjustedPathThenStop( robot.getSwerve(), @@ -1249,11 +1251,11 @@ private static PathPlannerPath getAllianceSideToStartingLinePath(AllianceSide al } - private static boolean hasStoppedThrowingBalls(Robot robot) { + public static boolean hasStoppedThrowingBalls(Robot robot) { return isTimeBetweenBallsAboveThreshold(robot, AutonomousConstants.TIME_BETWEEN_BALLS_TO_RETURN_TO_MIDDLE_SECONDS); } - private static boolean isReadyToCloseIntake(Robot robot) { + public static boolean isReadyToCloseIntake(Robot robot) { return isTimeBetweenBallsAboveThreshold(robot, AutonomousConstants.TIME_BETWEEN_BALLS_TO_CLOSE_INTAKE_SECONDS); } From ad911d3716bd0596ff328b8761d87800d7b0e78c Mon Sep 17 00:00:00 2001 From: greenblitz4590 <94316058+greenblitz4590@users.noreply.github.com> Date: Mon, 6 Jul 2026 11:13:44 +0300 Subject: [PATCH 6/8] nitay - it works --- src/main/java/frc/robot/autonomous/AutonomousConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/autonomous/AutonomousConstants.java b/src/main/java/frc/robot/autonomous/AutonomousConstants.java index 9adfa62c20..eab76b9f42 100644 --- a/src/main/java/frc/robot/autonomous/AutonomousConstants.java +++ b/src/main/java/frc/robot/autonomous/AutonomousConstants.java @@ -39,7 +39,7 @@ public class AutonomousConstants { public static final double TIME_BEFORE_AUTO_END_TO_RETURN_TO_MIDDLE_SECONDS_ON_OUTPOST_START = 4.0; public static final double TIME_BEFORE_AUTO_END_TO_RETURN_TO_MIDDLE_SECONDS_ON_DEPOT_START = 6.0; public static final double MINIMUM_TIME_AFTER_STARTING_TO_SHOOT_TO_RETURN_TO_MIDDLE = 1.0; - public static final double MINIMUM_TIME_AFTER_CLOSING_INTAKE_TO_RETURN_TO_MIDDLE = 0.3; + public static final double MINIMUM_TIME_AFTER_CLOSING_INTAKE_TO_RETURN_TO_MIDDLE = 1.3; public static final Double DEFAULT_STUCK_DEBOUNCE_SECONDS = 2.0; From 0051534359fe4539d1a99ccc3e897523307f5161 Mon Sep 17 00:00:00 2001 From: greenblitz4590 <94316058+greenblitz4590@users.noreply.github.com> Date: Mon, 6 Jul 2026 11:56:02 +0300 Subject: [PATCH 7/8] nitay - it works --- .../pathplanner/paths/R quarter light.path | 15 +----- src/main/java/frc/robot/Robot.java | 1 + .../robot/autonomous/AutonomousConstants.java | 2 +- .../frc/robot/autonomous/AutosBuilder.java | 46 ++++++++++++++++++- 4 files changed, 48 insertions(+), 16 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/R quarter light.path b/src/main/deploy/pathplanner/paths/R quarter light.path index 62fe720f3a..c8cbf2b828 100644 --- a/src/main/deploy/pathplanner/paths/R quarter light.path +++ b/src/main/deploy/pathplanner/paths/R quarter light.path @@ -111,7 +111,7 @@ }, { "waypointRelativePos": 3.8888980263157924, - "rotationDegrees": 45.0 + "rotationDegrees": 95.56836167213272 } ], "constraintZones": [ @@ -127,19 +127,6 @@ "nominalVoltage": 12.0, "unlimited": false } - }, - { - "name": "Constraints Zone", - "minWaypointRelativePos": 4.85, - "maxWaypointRelativePos": 5.5, - "constraints": { - "maxVelocity": 1.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } } ], "pointTowardsZones": [], diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 504e0fcd9e..e636315fa1 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -477,6 +477,7 @@ private void configureAuto() { autonomousOpenIntakeCommand, autonomousCloseIntakeCommand, autonomousScoringSequenceCommand, + () -> getRobotCommander().setState(RobotState.NEUTRAL), autonomousPassSequenceCommand, autonomousOuttakeCommand, AutonomousConstants.DEFAULT_PATHFINDING_CONSTRAINTS, diff --git a/src/main/java/frc/robot/autonomous/AutonomousConstants.java b/src/main/java/frc/robot/autonomous/AutonomousConstants.java index eab76b9f42..8f333baaad 100644 --- a/src/main/java/frc/robot/autonomous/AutonomousConstants.java +++ b/src/main/java/frc/robot/autonomous/AutonomousConstants.java @@ -39,7 +39,7 @@ public class AutonomousConstants { public static final double TIME_BEFORE_AUTO_END_TO_RETURN_TO_MIDDLE_SECONDS_ON_OUTPOST_START = 4.0; public static final double TIME_BEFORE_AUTO_END_TO_RETURN_TO_MIDDLE_SECONDS_ON_DEPOT_START = 6.0; public static final double MINIMUM_TIME_AFTER_STARTING_TO_SHOOT_TO_RETURN_TO_MIDDLE = 1.0; - public static final double MINIMUM_TIME_AFTER_CLOSING_INTAKE_TO_RETURN_TO_MIDDLE = 1.3; + public static final double MINIMUM_TIME_AFTER_CLOSING_INTAKE_TO_RETURN_TO_MIDDLE = 1.2; 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 fa44a425a0..5c7ac8a232 100644 --- a/src/main/java/frc/robot/autonomous/AutosBuilder.java +++ b/src/main/java/frc/robot/autonomous/AutosBuilder.java @@ -34,6 +34,7 @@ public static List> getAutoList( Supplier openIntake, Supplier closeIntake, Supplier scoreSequence, + Supplier dontScoreSequence, Supplier passSequence, Supplier outtakeSequence, PathConstraints pathfindingConstraints, @@ -49,6 +50,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -62,6 +64,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -75,6 +78,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -89,6 +93,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -103,6 +108,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -117,6 +123,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -130,6 +137,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -167,6 +175,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -183,6 +192,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -199,6 +209,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -215,6 +226,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -231,6 +243,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -247,6 +260,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -263,6 +277,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -279,6 +294,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -295,6 +311,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -311,6 +328,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -327,6 +345,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -343,6 +362,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -359,6 +379,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -373,6 +394,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -387,6 +409,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -401,6 +424,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -415,6 +439,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -429,6 +454,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -443,6 +469,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -457,6 +484,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -470,6 +498,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -486,6 +515,7 @@ private static Supplier getQuarterAuto( Supplier openIntake, Supplier closeIntake, Supplier scoreSequence, + Supplier dontScoreSequence, PathConstraints pathfindingConstraints, Pose2d regularIsNearEndOfPathTolerance, Pose2d stuckIsNearEndOfPathTolerance, @@ -527,6 +557,7 @@ private static Supplier getQuarterAuto( stuckDebounceSeconds, returnToMiddle, scoreSequence, + dontScoreSequence, closeIntake, openIntake ).asProxy() @@ -548,6 +579,7 @@ private static Supplier getHighTideAuto( Supplier openIntake, Supplier closeIntake, Supplier scoreSequence, + Supplier dontScoreSequence, PathConstraints pathfindingConstraints, Pose2d regularIsNearEndOfPathTolerance, Pose2d stuckIsNearEndOfPathTolerance, @@ -589,6 +621,7 @@ private static Supplier getHighTideAuto( stuckDebounceSeconds, returnToMiddle, scoreSequence, + dontScoreSequence, closeIntake, openIntake ).asProxy() @@ -613,6 +646,7 @@ private static Supplier getSideAuto( Supplier openIntake, Supplier closeIntake, Supplier scoreSequence, + Supplier dontScoreSequence, PathConstraints pathfindingConstraints, Pose2d regularIsNearEndOfPathTolerance, Pose2d stuckIsNearEndOfPathTolerance, @@ -685,6 +719,7 @@ private static Supplier getSideAuto( stuckDebounceSeconds, returnToMiddle, scoreSequence, + dontScoreSequence, closeIntake, openIntake ) @@ -729,6 +764,7 @@ private static Supplier getStealAuto( Supplier openIntake, Supplier closeIntake, Supplier scoreSequence, + Supplier dontScoreSequence, PathConstraints pathfindingConstraints, Pose2d regularIsNearEndOfPathTolerance, Pose2d stuckIsNearEndOfPathTolerance, @@ -800,6 +836,7 @@ private static Supplier getStealAuto( stuckDebounceSeconds, returnToMiddle, scoreSequence, + dontScoreSequence, closeIntake, openIntake ) @@ -829,6 +866,7 @@ private static Supplier getLightStealAuto( Supplier openIntake, Supplier closeIntake, Supplier scoreSequence, + Supplier dontScoreSequence, PathConstraints pathfindingConstraints, Pose2d regularIsNearEndOfPathTolerance, Pose2d stuckIsNearEndOfPathTolerance, @@ -899,6 +937,7 @@ private static Supplier getLightStealAuto( stuckDebounceSeconds, returnToMiddle, scoreSequence, + dontScoreSequence, closeIntake, openIntake ) @@ -928,6 +967,7 @@ private static Supplier getLightQuarterAuto( Supplier openIntake, Supplier closeIntake, Supplier scoreSequence, + Supplier dontScoreSequence, PathConstraints pathfindingConstraints, Pose2d regularIsNearEndOfPathTolerance, Pose2d stuckIsNearEndOfPathTolerance, @@ -972,6 +1012,7 @@ private static Supplier getLightQuarterAuto( stuckDebounceSeconds, returnToMiddle, scoreSequence, + dontScoreSequence, closeIntake, openIntake @@ -1001,6 +1042,7 @@ private static Supplier getHorseshoeAuto( Supplier openIntake, Supplier closeIntake, Supplier scoreSequence, + Supplier dontScoreSequence, PathConstraints pathfindingConstraints, Pose2d regularIsNearEndOfPathTolerance, Pose2d stuckIsNearEndOfPathTolerance, @@ -1051,6 +1093,7 @@ private static Supplier getHorseshoeAuto( stuckDebounceSeconds, returnToMiddle, scoreSequence, + dontScoreSequence, closeIntake, openIntake ) @@ -1194,6 +1237,7 @@ private static Command getAllianceSideToStartingLineAuto( double stuckDebounceSeconds, BooleanSupplier returnToMiddle, Supplier scoreSequence, + Supplier dontScoreSequence, Supplier closeIntake, Supplier openIntake ) { @@ -1221,7 +1265,7 @@ private static Command getAllianceSideToStartingLineAuto( ).until(isReadyToReturnToMiddle(returnToMiddle, allianceSide)) .andThen( new ParallelCommandGroup( - scoreSequence.get(), + dontScoreSequence.get(), openIntake.get(), PathFollowingCommandsBuilder.followAdjustedPathThenStop( robot.getSwerve(), From 1af2cc4b7adae1a26548d110b231460855c82610 Mon Sep 17 00:00:00 2001 From: greenblitz4590 <94316058+greenblitz4590@users.noreply.github.com> Date: Mon, 6 Jul 2026 12:18:23 +0300 Subject: [PATCH 8/8] nitay - works --- src/main/java/frc/robot/Robot.java | 2 ++ src/main/java/frc/robot/autonomous/AutosBuilder.java | 11 +++++++++-- 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e636315fa1..39cc4aad9a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -16,6 +16,7 @@ import edu.wpi.first.wpilibj2.command.*; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.RobotManager; +import frc.constants.field.AllianceSide; import frc.robot.autonomous.AutonomousConstants; import frc.robot.autonomous.AutosBuilder; import frc.robot.hardware.digitalinput.IDigitalInput; @@ -344,6 +345,7 @@ public void periodic() { Logger.recordOutput("IsReadyToCloseIntakeForAutonomous", AutosBuilder.isReadyToCloseIntake(this)); Logger.recordOutput("HasStoppedThrowingBallsForAutonomous", AutosBuilder.hasStoppedThrowingBalls(this)); + AutosBuilder.isReadyToReturnToMiddle(() -> returnToMiddle.getSelected() != null && returnToMiddle.getSelected(), AllianceSide.DEPOT); BatteryUtil.logStatus(); BusChain.logChainsStatuses(); CommandScheduler.getInstance().run(); // Should be last diff --git a/src/main/java/frc/robot/autonomous/AutosBuilder.java b/src/main/java/frc/robot/autonomous/AutosBuilder.java index 5c7ac8a232..3a3fa46884 100644 --- a/src/main/java/frc/robot/autonomous/AutosBuilder.java +++ b/src/main/java/frc/robot/autonomous/AutosBuilder.java @@ -1262,7 +1262,7 @@ private static Command getAllianceSideToStartingLineAuto( ) ), scoreSequence.get() - ).until(isReadyToReturnToMiddle(returnToMiddle, allianceSide)) + ).until(() -> isReadyToReturnToMiddle(returnToMiddle, allianceSide).getAsBoolean()) .andThen( new ParallelCommandGroup( dontScoreSequence.get(), @@ -1311,7 +1311,14 @@ private static boolean isTimeBetweenBallsAboveThreshold(Robot robot, double thre return TimeUtil.getCurrentTimeSeconds() - lastBallTimestamp > threshold; } - private static BooleanSupplier isReadyToReturnToMiddle(BooleanSupplier returnToMiddleByTimer, AllianceSide allianceSide) { + public static BooleanSupplier isReadyToReturnToMiddle(BooleanSupplier returnToMiddleByTimer, AllianceSide allianceSide) { + Logger.recordOutput("isReadyToReturnToMiddleForAutonomus", (returnToMiddleByTimer.getAsBoolean() + && TimeUtil.getCurrentTimeSeconds() - TimeUtil.getAutonomousStartTimeSeconds() + > GamePeriodUtils.AUTONOMOUS_DURATION_SECONDS + - (allianceSide == AllianceSide.OUTPOST + ? AutonomousConstants.TIME_BEFORE_AUTO_END_TO_RETURN_TO_MIDDLE_SECONDS_ON_OUTPOST_START + : AutonomousConstants.TIME_BEFORE_AUTO_END_TO_RETURN_TO_MIDDLE_SECONDS_ON_DEPOT_START))); + return () -> (returnToMiddleByTimer.getAsBoolean() && TimeUtil.getCurrentTimeSeconds() - TimeUtil.getAutonomousStartTimeSeconds() > GamePeriodUtils.AUTONOMOUS_DURATION_SECONDS