diff --git a/src/main/deploy/pathplanner/paths/Depot - Middle.path b/src/main/deploy/pathplanner/paths/Depot - Middle.path index 7671e3b431..b24c6e901e 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": 0.698, - "y": 5.973 + "x": 2.690439404677534, + "y": 5.375251594613749 }, "prevControl": null, "nextControl": { - "x": 1.698514684606482, - "y": 5.874260054976852 + "x": 3.6479001750056397, + "y": 5.491699526140139 }, "isLocked": false, - "linkedName": null + "linkedName": "Depot" }, { "anchor": { @@ -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..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.11212048192771, - "y": 5.712403614457831 + "x": 3.2498002853067045, + "y": 5.619985734664765 }, "prevControl": { - "x": 1.7753156184874421, - "y": 5.508890246069346 + "x": 1.8736569788426172, + "y": 5.889850308509359 }, "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": -34.70155660178564 + "rotation": -139.1849161256499 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": 0.9279887127821316 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ 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..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": 1.904179743223965, - "y": 2.500000000000001 + "x": 2.6390148830616584, + "y": 2.3733451452870304 }, "prevControl": { - "x": 2.4666588375526692, - "y": 3.0030359519675933 + "x": 3.2014939773903626, + "y": 2.876381097254623 }, "nextControl": null, "isLocked": false, @@ -177,7 +177,7 @@ }, "goalEndState": { "velocity": 0.0, - "rotation": 80.579832280714 + "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 e51a1422c8..8f31e499d0 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": 2.908993621545004, + "y": 2.5790432317505316 }, "prevControl": null, "nextControl": { - "x": 3.2498002853067045, - "y": 2.3076890156918695 + "x": 4.254614163627744, + "y": 2.3867322474424 }, "isLocked": false, - "linkedName": "Outpost alliance wait" + "linkedName": "Outpost middle return" }, { "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": 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 new file mode 100644 index 0000000000..7e8351d0c0 --- /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": 2.6390148830616584, + "y": 2.3733451452870304 + }, + "prevControl": { + "x": 13.106390061378349, + "y": 2.3233594106222655 + }, + "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": 130.9800700830008 + }, + "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..c8cbf2b828 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": 2.6390148830616584, + "y": 2.3733451452870304 }, "prevControl": { - "x": 2.1541797345840688, - "y": 2.4999342736871943 + "x": 2.889014874421762, + "y": 2.373279418974224 }, "nextControl": null, "isLocked": false, @@ -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": [], @@ -154,7 +141,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 80.579832280714 + "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 ad2d863932..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": 1.904179743223965, - "y": 2.500000000000001 + "x": 2.6390148830616584, + "y": 2.3733451452870304 }, "prevControl": { - "x": 2.4938613897909025, - "y": 2.500000000000001 + "x": 3.228696529628596, + "y": 2.3733451452870304 }, "nextControl": null, "isLocked": false, @@ -156,7 +156,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 80.579832280714 + "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..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; @@ -341,6 +342,10 @@ public void periodic() { GamePeriodUtils.log(); + 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 @@ -474,6 +479,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 07a0d60152..8f333baaad 100644 --- a/src/main/java/frc/robot/autonomous/AutonomousConstants.java +++ b/src/main/java/frc/robot/autonomous/AutonomousConstants.java @@ -34,10 +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_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; + 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 c9dcb74fbc..3a3fa46884 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; @@ -33,6 +34,7 @@ public static List> getAutoList( Supplier openIntake, Supplier closeIntake, Supplier scoreSequence, + Supplier dontScoreSequence, Supplier passSequence, Supplier outtakeSequence, PathConstraints pathfindingConstraints, @@ -48,6 +50,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -61,6 +64,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -74,6 +78,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -88,6 +93,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -102,6 +108,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -110,24 +117,13 @@ public static List> getAutoList( true, returnToMiddle ), - getExtendedLQuarterAuto( - robot, - resetSubsystems, - openIntake, - closeIntake, - scoreSequence, - pathfindingConstraints, - regularIsNearEndOfPathTolerance, - stuckIsNearEndOfPathTolerance, - stuckDebounceSeconds, - returnToMiddle - ), getHorseshoeAuto( robot, resetSubsystems, openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -141,6 +137,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -178,6 +175,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -194,6 +192,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -210,6 +209,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -226,6 +226,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -242,6 +243,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -258,6 +260,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -274,6 +277,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -290,6 +294,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -306,6 +311,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -322,6 +328,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -338,6 +345,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -354,6 +362,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -370,6 +379,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -384,6 +394,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -398,6 +409,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -412,6 +424,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -426,6 +439,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -440,6 +454,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -454,6 +469,7 @@ public static List> getAutoList( openIntake, closeIntake, scoreSequence, + dontScoreSequence, pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, @@ -461,6 +477,34 @@ public static List> getAutoList( AllianceSide.OUTPOST, true, returnToMiddle + ), + getHighTideAuto( + robot, + resetSubsystems, + openIntake, + closeIntake, + scoreSequence, + dontScoreSequence, + pathfindingConstraints, + regularIsNearEndOfPathTolerance, + stuckIsNearEndOfPathTolerance, + stuckDebounceSeconds, + AllianceSide.OUTPOST, + returnToMiddle + ), + getHighTideAuto( + robot, + resetSubsystems, + openIntake, + closeIntake, + scoreSequence, + dontScoreSequence, + pathfindingConstraints, + regularIsNearEndOfPathTolerance, + stuckIsNearEndOfPathTolerance, + stuckDebounceSeconds, + AllianceSide.DEPOT, + returnToMiddle ) ); } @@ -471,6 +515,7 @@ private static Supplier getQuarterAuto( Supplier openIntake, Supplier closeIntake, Supplier scoreSequence, + Supplier dontScoreSequence, PathConstraints pathfindingConstraints, Pose2d regularIsNearEndOfPathTolerance, Pose2d stuckIsNearEndOfPathTolerance, @@ -496,6 +541,70 @@ private static Supplier getQuarterAuto( .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, + dontScoreSequence, + closeIntake, + openIntake + ).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, + Supplier dontScoreSequence, + 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 High Tide"), + pathfindingConstraints, + regularIsNearEndOfPathTolerance, + stuckIsNearEndOfPathTolerance, + stuckDebounceSeconds, + robot.getSwerve().getLogPath() + ) + .asProxy() + .alongWith(new InstantCommand(() -> hasPathEnded = false)) + .andThen(new InstantCommand(() -> hasPathEnded = true)), new SequentialCommandGroup( resetSubsystems.get(), new ParallelCommandGroup( @@ -512,27 +621,32 @@ private static Supplier getQuarterAuto( stuckDebounceSeconds, returnToMiddle, scoreSequence, + dontScoreSequence, closeIntake, - openIntake, - true + openIntake ).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, returnToMiddle, true) + 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) ); } + private static Supplier getSideAuto( Robot robot, Supplier resetSubsystems, Supplier openIntake, Supplier closeIntake, Supplier scoreSequence, + Supplier dontScoreSequence, PathConstraints pathfindingConstraints, Pose2d regularIsNearEndOfPathTolerance, Pose2d stuckIsNearEndOfPathTolerance, @@ -605,15 +719,14 @@ private static Supplier getSideAuto( stuckDebounceSeconds, returnToMiddle, scoreSequence, + dontScoreSequence, closeIntake, - openIntake, - false + openIntake ) ) .asProxy(), new WaitCommand(AutonomousConstants.TIME_TO_WAIT_TO_CLOSE_INTAKE_AFTER_PATH_END_SECONDS) .andThen(closeIntake.get()) - .onlyIf(() -> !returnToMiddle.getAsBoolean()) ) ) ) @@ -651,6 +764,7 @@ private static Supplier getStealAuto( Supplier openIntake, Supplier closeIntake, Supplier scoreSequence, + Supplier dontScoreSequence, PathConstraints pathfindingConstraints, Pose2d regularIsNearEndOfPathTolerance, Pose2d stuckIsNearEndOfPathTolerance, @@ -722,15 +836,14 @@ private static Supplier getStealAuto( stuckDebounceSeconds, returnToMiddle, scoreSequence, + dontScoreSequence, closeIntake, - openIntake, - false + openIntake ) ) .asProxy(), new WaitCommand(AutonomousConstants.TIME_TO_WAIT_TO_CLOSE_INTAKE_AFTER_PATH_END_SECONDS) .andThen(closeIntake.get()) - .onlyIf(() -> !returnToMiddle.getAsBoolean()) ) ) ) @@ -742,7 +855,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) ); } @@ -752,6 +866,7 @@ private static Supplier getLightStealAuto( Supplier openIntake, Supplier closeIntake, Supplier scoreSequence, + Supplier dontScoreSequence, PathConstraints pathfindingConstraints, Pose2d regularIsNearEndOfPathTolerance, Pose2d stuckIsNearEndOfPathTolerance, @@ -822,15 +937,14 @@ private static Supplier getLightStealAuto( stuckDebounceSeconds, returnToMiddle, scoreSequence, + dontScoreSequence, closeIntake, - openIntake, - false + openIntake ) ) .asProxy(), new WaitCommand(AutonomousConstants.TIME_TO_WAIT_TO_CLOSE_INTAKE_AFTER_PATH_END_SECONDS) .andThen(closeIntake.get()) - .onlyIf(() -> !returnToMiddle.getAsBoolean()) ) ) ) @@ -853,6 +967,7 @@ private static Supplier getLightQuarterAuto( Supplier openIntake, Supplier closeIntake, Supplier scoreSequence, + Supplier dontScoreSequence, PathConstraints pathfindingConstraints, Pose2d regularIsNearEndOfPathTolerance, Pose2d stuckIsNearEndOfPathTolerance, @@ -888,25 +1003,23 @@ private static Supplier getLightQuarterAuto( openIntake.get() .until(() -> hasPathEnded) .andThen( - new ParallelCommandGroup( + getAllianceSideToStartingLineAuto( + robot, + startingSide, + pathfindingConstraints, + regularIsNearEndOfPathTolerance, + stuckIsNearEndOfPathTolerance, + stuckDebounceSeconds, + returnToMiddle, + scoreSequence, + dontScoreSequence, + closeIntake, + openIntake + + ).asProxy(), + new WaitCommand(AutonomousConstants.TIME_TO_WAIT_TO_CLOSE_INTAKE_AFTER_PATH_END_SECONDS) + .andThen(closeIntake.get()) - getAllianceSideToStartingLineAuto( - robot, - startingSide, - pathfindingConstraints, - regularIsNearEndOfPathTolerance, - stuckIsNearEndOfPathTolerance, - stuckDebounceSeconds, - returnToMiddle, - scoreSequence, - closeIntake, - openIntake, - true - ).asProxy(), - new WaitCommand(AutonomousConstants.TIME_TO_WAIT_TO_CLOSE_INTAKE_AFTER_PATH_END_SECONDS) - .andThen(closeIntake.get()) - .onlyIf(() -> !returnToMiddle.getAsBoolean()) - ) ) ) ) @@ -918,81 +1031,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, - openIntake, - 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) ); } @@ -1002,6 +1042,7 @@ private static Supplier getHorseshoeAuto( Supplier openIntake, Supplier closeIntake, Supplier scoreSequence, + Supplier dontScoreSequence, PathConstraints pathfindingConstraints, Pose2d regularIsNearEndOfPathTolerance, Pose2d stuckIsNearEndOfPathTolerance, @@ -1052,15 +1093,14 @@ private static Supplier getHorseshoeAuto( stuckDebounceSeconds, returnToMiddle, scoreSequence, + dontScoreSequence, closeIntake, - openIntake, - false + openIntake ) ) .asProxy(), new WaitCommand(AutonomousConstants.TIME_TO_WAIT_TO_CLOSE_INTAKE_AFTER_PATH_END_SECONDS) .andThen(closeIntake.get()) - .onlyIf(() -> !returnToMiddle.getAsBoolean()) ) ) ) @@ -1071,7 +1111,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) ); } @@ -1196,70 +1237,95 @@ private static Command getAllianceSideToStartingLineAuto( double stuckDebounceSeconds, BooleanSupplier returnToMiddle, Supplier scoreSequence, + Supplier dontScoreSequence, Supplier closeIntake, - Supplier openIntake, - boolean isQuarter + 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(() -> hasStoppedThrowingBalls(robot))) - .andThen(closeIntake.get()), - 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) - ) - .andThen( - new ParallelCommandGroup( - scoreSequence.get(), - openIntake.get(), - PathFollowingCommandsBuilder - .followAdjustedPathThenStop( + .andThen(new RunCommand(() -> {}).until(() -> isReadyToCloseIntake(robot))) + .andThen( + 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, returnToMiddle, isQuarter), + getAllianceSideToStartingLinePath(allianceSide), pathfindingConstraints, regularIsNearEndOfPathTolerance, stuckIsNearEndOfPathTolerance, stuckDebounceSeconds, robot.getSwerve().getLogPath() ) - .andThen( - robot.getSwerve() - .getCommandsBuilder() - .wiggle(AutonomousConstants.WIGGLE_RANGE, AutonomousConstants.TIME_BETWEEN_WIGGLES_SECONDS) - .onlyIf(() -> !returnToMiddle.getAsBoolean()) - ) + ) + ), + scoreSequence.get() + ).until(() -> isReadyToReturnToMiddle(returnToMiddle, allianceSide).getAsBoolean()) + .andThen( + new ParallelCommandGroup( + dontScoreSequence.get(), + openIntake.get(), + PathFollowingCommandsBuilder.followAdjustedPathThenStop( + robot.getSwerve(), + () -> robot.getPoseEstimator().getEstimatedPose(), + getAllianceSideToMiddlePath(allianceSide), + pathfindingConstraints, + regularIsNearEndOfPathTolerance, + stuckIsNearEndOfPathTolerance, + stuckDebounceSeconds, + robot.getSwerve().getLogPath() + ) ) ); } - 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"); - } + 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"); } - private static boolean hasStoppedThrowingBalls(Robot robot) { + public static boolean hasStoppedThrowingBalls(Robot robot) { + return isTimeBetweenBallsAboveThreshold(robot, AutonomousConstants.TIME_BETWEEN_BALLS_TO_RETURN_TO_MIDDLE_SECONDS); + } + + public 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; + } + + 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 + - (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)); } + }