Only allow coast when override is switched during disable

Mechanical-Advantage · Mar 7, 2024 · 3df8355 · 3df8355
1 parent 5774ee3
commit 3df8355
Showing 1 changed file with 11 additions and 2 deletions.
diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java
@@ -117,6 +117,7 @@ public class RobotContainer {
 
   private boolean podiumShotMode = false;
   private boolean trapScoreMode = true;
+  private boolean coastOverride = false;
 
   // Dashboard inputs
   private final LoggedDashboardChooser<Command> autoChooser =
@@ -257,9 +258,17 @@ public RobotContainer() {
     superstructure = new Superstructure(arm, climber, backpackActuator);
 
     // Set up subsystems
-    arm.setOverrides(armDisable, armCoast);
+    // Set up coast override
+    armCoast
+        .and(DriverStation::isDisabled)
+        .onTrue(Commands.runOnce(() -> coastOverride = true).ignoringDisable(true));
+    armCoast
+        .negate()
+        .or(DriverStation::isEnabled)
+        .onTrue(Commands.runOnce(() -> coastOverride = false).ignoringDisable(true));
+    arm.setOverrides(armDisable, () -> coastOverride);
+    climber.setCoastOverride(() -> coastOverride);
     RobotState.getInstance().setLookaheadDisable(lookaheadDisable);
-    climber.setCoastOverride(armCoast);
     flywheels.setPrepareShootSupplier(
         () -> {
           return DriverStation.isTeleopEnabled()