fix brake mode (#43)

* feat: add usable brake mode logic * fix: remove enabledInit, add FMS check for isPreMatch
flamingchickens1540 · Apr 2, 2024 · 70a7d20 · 70a7d20
1 parent 8090624
commit 70a7d20
Show file tree

Hide file tree

Showing 7 changed files with 65 additions and 50 deletions.
diff --git a/src/main/java/org/team1540/robot2024/Robot.java b/src/main/java/org/team1540/robot2024/Robot.java
@@ -98,8 +98,7 @@ public void robotInit() {
         // and put our autonomous chooser on the dashboard.
         robotContainer = new RobotContainer();
 
-        robotContainer.shooter.setPivotBrakeMode(false);
-        robotContainer.drivetrain.setBrakeMode(false);
+        robotContainer.disableBrakeMode();
 
         // Pathplanner warmup (helps prevents delays at the start of auto)
         FollowPathCommand.warmupCommand().schedule();
@@ -134,9 +133,6 @@ public void robotPeriodic() {
      */
     @Override
     public void disabledInit() {
-//        robotContainer.elevator.setBrakeMode(false);
-        robotContainer.shooter.setPivotBrakeMode(false);
-        robotContainer.indexer.setIntakeNeutralMode(false);
         robotContainer.drivetrain.unblockTags();
         robotContainer.shooter.setPivotPosition(new Rotation2d());
         robotContainer.leds.setPattern(Leds.Zone.MAIN, new LedPatternRainbow(1));
@@ -151,19 +147,11 @@ public void disabledPeriodic() {
         AutoManager.getInstance().updateSelected();
     }
 
-    public void enabledInit() {
-        robotContainer.elevator.setBrakeMode(true);
-        robotContainer.shooter.setPivotBrakeMode(true);
-        robotContainer.drivetrain.setBrakeMode(true);
-        robotContainer.indexer.setIntakeNeutralMode(true);
-    }
-
     /**
      * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
      */
     @Override
     public void autonomousInit() {
-
 //        robotContainer.leds.setPatternAll(LedPatternFlame::new, Leds.PatternCriticality.HIGH);
         robotContainer.drivetrain.blockTags();
 
@@ -176,19 +164,26 @@ public void autonomousInit() {
 
     }
 
+    @Override
+    public void autonomousExit() {
+        robotContainer.enableBrakeMode(true);
+    }
+
     /**
      * This function is called periodically during autonomous.
      */
     @Override
     public void autonomousPeriodic() {
     }
 
+
+
     /**
      * This function is called once when teleop is enabled.
      */
     @Override
     public void teleopInit() {
-        enabledInit();
+        robotContainer.enableBrakeMode(false);
 //        robotContainer.leds.setPatternAll(() -> new LedPatternRainbow(2), Leds.PatternCriticality.HIGH);
         robotContainer.drivetrain.zeroFieldOrientation();// TODO: remove this once odometry / startup zero is good
 
@@ -197,6 +192,10 @@ public void teleopInit() {
         }
     }
 
+    @Override
+    public void teleopExit() {
+        robotContainer.enableBrakeMode(true);
+    }
     /**
      * This function is called periodically during operator control.
      */
@@ -209,9 +208,6 @@ public void teleopPeriodic() {
      */
     @Override
     public void testInit() {
-        robotContainer.leds.setPattern(Leds.Zone.MAIN,new LedPatternTuneColor());
-        // Cancels all running commands at the start of test mode.
-        CommandScheduler.getInstance().cancelAll();
     }
 
     /**

diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java
@@ -4,12 +4,11 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.RobotController;
-import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
+import org.littletonrobotics.junction.Logger;
 import org.team1540.robot2024.commands.FeedForwardCharacterization;
 import org.team1540.robot2024.commands.autos.*;
 import org.team1540.robot2024.commands.climb.ClimbAlignment;
@@ -27,10 +26,7 @@
 import org.team1540.robot2024.subsystems.elevator.Elevator;
 import org.team1540.robot2024.subsystems.indexer.Indexer;
 import org.team1540.robot2024.subsystems.led.Leds;
-import org.team1540.robot2024.subsystems.led.patterns.LedPatternProgressBar;
-import org.team1540.robot2024.subsystems.led.patterns.LedPatternRSLState;
-import org.team1540.robot2024.subsystems.led.patterns.LedPatternWave;
-import org.team1540.robot2024.subsystems.led.patterns.SimpleLedPattern;
+import org.team1540.robot2024.subsystems.led.patterns.*;
 import org.team1540.robot2024.subsystems.shooter.Shooter;
 import org.team1540.robot2024.subsystems.tramp.Tramp;
 import org.team1540.robot2024.subsystems.vision.AprilTagVision;
@@ -39,6 +35,8 @@
 import org.team1540.robot2024.util.auto.AutoCommand;
 import org.team1540.robot2024.util.auto.AutoManager;
 
+import java.util.function.BooleanSupplier;
+
 import static org.team1540.robot2024.Constants.SwerveConfig;
 import static org.team1540.robot2024.Constants.isTuningMode;
 
@@ -58,6 +56,9 @@ public class RobotContainer {
 
     public final PhoenixTimeSyncSignalRefresher odometrySignalRefresher = new PhoenixTimeSyncSignalRefresher(SwerveConfig.CAN_BUS);
 
+
+
+    public boolean isBrakeMode;
     /**
      * The container for the robot. Contains subsystems, IO devices, and commands.
      */
@@ -130,7 +131,7 @@ private void configureButtonBindings() {
         driver.b().onTrue(Commands.runOnce(drivetrain::stopWithX, drivetrain));
         driver.y().onTrue(Commands.runOnce(() -> {
             drivetrain.zeroFieldOrientationManual();
-            drivetrain.setBrakeMode(true);
+            enableBrakeMode(false);
         }).ignoringDisable(true));
 
         Command targetDrive = new AutoShootPrepareWithTargeting(driver.getHID(), drivetrain, shooter)
@@ -214,17 +215,24 @@ private void configureButtonBindings() {
 
         new Trigger(indexer::isNoteStaged).and(intakeCommand::isScheduled).onTrue(CommandUtils.rumbleCommandTimed(driver.getHID(), 0.8, 0.4));
 
-        new Trigger(RobotController::getUserButton).toggleOnTrue(Commands.startEnd(
-                () -> {
-                    elevator.setBrakeMode(false);
-                    leds.setPatternAll(() -> new LedPatternRSLState(Color.kMagenta), Leds.PatternLevel.ELEVATOR_STATE);
-                },
+
+        BooleanSupplier isPreMatch = () -> (!DriverStation.isDSAttached() || !DriverStation.isFMSAttached() || DriverStation.isAutonomous()) && DriverStation.isDisabled();
+        Command brakeModeCommand = Commands.runOnce(
                 () -> {
-                    elevator.setBrakeMode(true);
-                    leds.clearPatternAll(Leds.PatternLevel.ELEVATOR_STATE);
+                    if (!isPreMatch.getAsBoolean()) {return;}
+                    if (!isBrakeMode) {
+                        enableBrakeMode(true);
+                    } else {
+                        disableBrakeMode();
+                    }
                 }
-        ).ignoringDisable(true));
+        ).ignoringDisable(true);
+
+        new Trigger(tramp::isNoteStaged).debounce(0.1)
+                .onTrue(brakeModeCommand);
 
+        driver.start().onTrue(brakeModeCommand);
+        copilot.start().onTrue(brakeModeCommand);
 
     }
 
@@ -286,4 +294,27 @@ private void configureAutoRoutines() {
     public Command getAutonomousCommand() {
         return AutoManager.getInstance().getSelected();
     }
+
+    public void enableBrakeMode(boolean requireRobotDisabled) {
+        if (!isBrakeMode && (!requireRobotDisabled || !DriverStation.isEnabled())) {
+            shooter.setPivotBrakeMode(true);
+            indexer.setIntakeBrakeMode(true);
+            elevator.setBrakeMode(true);
+            drivetrain.setBrakeMode(true);
+            leds.clearPattern(Leds.Zone.MAIN, Leds.PatternLevel.COAST_STATE);
+            isBrakeMode = true;
+            Logger.recordOutput("brakeMode", true);
+        }
+    }
+    public void disableBrakeMode() {
+        if (isBrakeMode && !DriverStation.isEnabled()) {
+            shooter.setPivotBrakeMode(false);
+            indexer.setIntakeBrakeMode(false);
+            elevator.setBrakeMode(false);
+            drivetrain.setBrakeMode(false);
+            leds.setPattern(Leds.Zone.MAIN, new LedPatternBreathing("#f000c7"), Leds.PatternLevel.COAST_STATE);
+            isBrakeMode = false;
+            Logger.recordOutput("brakeMode", false);
+        }
+    }
 }
diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java
@@ -133,8 +133,8 @@ public Command commandRunIntake(double percent) {
                 );
     }
 
-    public void setIntakeNeutralMode(boolean isBrake) {
-        io.setIntakeNeutralMode(isBrake);
+    public void setIntakeBrakeMode(boolean isBrake) {
+        io.setIntakeBrakeMode(isBrake);
     }
 
 }
diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java
@@ -30,6 +30,6 @@ default void setFeederVelocity(double velocityRPM) {}
 
     default void configureFeederPID(double p, double i, double d) {}
 
-    default void setIntakeNeutralMode(boolean isBrakeMode) {}
+    default void setIntakeBrakeMode(boolean isBrakeMode) {}
 
 }
diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java
@@ -74,7 +74,7 @@ public void configureFeederPID(double p, double i, double d) {
         feederPID.setD(d);
     }
 
-    public void setIntakeNeutralMode(boolean isBrakeMode) {
+    public void setIntakeBrakeMode(boolean isBrakeMode) {
         intakeMotor.setIdleMode(isBrakeMode ? CANSparkBase.IdleMode.kBrake : CANSparkBase.IdleMode.kCoast);
     }
 }

diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOTalonFX.java
@@ -116,7 +116,7 @@ public void configureFeederPID(double p, double i, double d) {
     }
 
     @Override
-    public void setIntakeNeutralMode(boolean isBrakeMode) {
+    public void setIntakeBrakeMode(boolean isBrakeMode) {
         intakeMotor.setNeutralMode(isBrakeMode ? NeutralModeValue.Brake : NeutralModeValue.Coast);
     }
 }
diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java b/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java
@@ -62,19 +62,7 @@ public void clearPattern(Zone zone, PatternLevel priority) {
         patterns[zone.ordinal()].clearPattern(priority);
     }
 
-    public void setPatternAll(Supplier<LedPattern> patternSupplier, PatternLevel priority) {
-        for (int i = 0; i<ZONE_COUNT;i++) {
-            LedPattern pattern = patternSupplier.get();
-            patterns[i].addPattern(pattern, priority);
-            pattern.setLength(buffers[i].getLength());
-        }
-    }
 
-    public void clearPatternAll(PatternLevel priority) {
-        for (int i = 0; i<ZONE_COUNT;i++) {
-            patterns[i].clearPattern(priority);
-        }
-    }
 
 
     private static final int ZONE_COUNT=Zone.values().length;
@@ -87,8 +75,8 @@ public enum PatternLevel {
         DEFAULT,
         TRAMP_STATE,
         INTAKE_STATE,
+        COAST_STATE,
         DRIVER_LOCK,
-        ELEVATOR_STATE
     }
 
     public Command commandShowPattern(LedPattern pattern, PatternLevel priority) {