Add back drive coast using threads (#230)

* Fix drive coast on Talons and add test auto

* Remove coast test auto

src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java

@@ -24,7 +24,7 @@
 import java.util.concurrent.locks.ReentrantLock;
 import java.util.function.Supplier;
 import java.util.stream.IntStream;
-import lombok.Getter;
+import lombok.Setter;
 import lombok.experimental.ExtensionMethod;
 import org.littletonrobotics.frc2024.Constants;
 import org.littletonrobotics.frc2024.RobotState;
@@ -51,7 +51,7 @@ public class Drive extends SubsystemBase {
   private static final LoggedTunableNumber coastMetersPerSecThreshold =
       new LoggedTunableNumber("Drive/CoastMetersPerSecThreshold", 0.05);
 
-  public enum DriveMode {
+  public static enum DriveMode {
     /** Driving with input from driver joysticks. (Default) */
     TELEOP,
 
@@ -68,6 +68,12 @@ public enum DriveMode {
     WHEEL_RADIUS_CHARACTERIZATION
   }
 
+  public static enum CoastRequest {
+    AUTOMATIC,
+    ALWAYS_BRAKE,
+    ALWAYS_COAST
+  }
+
   @AutoLog
   public static class OdometryTimestampInputs {
     public double[] timestamps = new double[] {};
@@ -93,10 +99,15 @@ public static class OdometryTimestampInputs {
   private boolean modulesOrienting = false;
   private final Timer lastMovementTimer = new Timer();
 
-  @Getter
   @AutoLogOutput(key = "Drive/BrakeModeEnabled")
   private boolean brakeModeEnabled = true;
 
+  @Setter
+  @AutoLogOutput(key = "Drive/CoastRequest")
+  private CoastRequest coastRequest = CoastRequest.AUTOMATIC;
+
+  private boolean lastEnabled = false;
+
   private ChassisSpeeds desiredSpeeds = new ChassisSpeeds();
 
   private SwerveSetpoint currentSetpoint =
@@ -122,6 +133,7 @@ public Drive(GyroIO gyroIO, ModuleIO fl, ModuleIO fr, ModuleIO bl, ModuleIO br)
     modules[2] = new Module(bl, 2);
     modules[3] = new Module(br, 3);
     lastMovementTimer.start();
+    setBrakeMode(true);
 
     setpointGenerator =
         SwerveSetpointGenerator.builder()
@@ -220,6 +232,26 @@ public void periodic() {
         .anyMatch(module -> module.getVelocityMetersPerSec() > coastMetersPerSecThreshold.get())) {
       lastMovementTimer.reset();
     }
+    if (DriverStation.isEnabled() && !lastEnabled) {
+      coastRequest = CoastRequest.AUTOMATIC;
+    }
+
+    lastEnabled = DriverStation.isEnabled();
+    switch (coastRequest) {
+      case AUTOMATIC -> {
+        if (DriverStation.isEnabled()) {
+          setBrakeMode(true);
+        } else if (lastMovementTimer.hasElapsed(coastWaitTime.get())) {
+          setBrakeMode(false);
+        }
+      }
+      case ALWAYS_BRAKE -> {
+        setBrakeMode(true);
+      }
+      case ALWAYS_COAST -> {
+        setBrakeMode(false);
+      }
+    }
 
     // Run drive based on current mode
     ChassisSpeeds teleopSpeeds = teleopDriveController.update();
@@ -401,6 +433,14 @@ public double[] getWheelRadiusCharacterizationPosition() {
     return Arrays.stream(modules).mapToDouble(Module::getPositionRads).toArray();
   }
 
+  /** Set brake mode to {@code enabled} doesn't change brake mode if already set. */
+  private void setBrakeMode(boolean enabled) {
+    if (brakeModeEnabled != enabled) {
+      Arrays.stream(modules).forEach(module -> module.setBrakeMode(enabled));
+    }
+    brakeModeEnabled = enabled;
+  }
+
   /**
    * Returns command that orients all modules to {@code orientation}, ending when the modules have
    * rotated.

src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Module.java

@@ -94,6 +94,12 @@ public void runCharacterization(double turnSetpointRads, double input) {
     io.runCharacterization(input);
   }
 
+  /** Sets brake mode to {@code enabled}. */
+  public void setBrakeMode(boolean enabled) {
+    io.setDriveBrakeMode(enabled);
+    io.setTurnBrakeMode(enabled);
+  }
+
   /** Stops motors. */
   public void stop() {
     io.stop();

src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOKrakenFOC.java

@@ -22,6 +22,8 @@
 import edu.wpi.first.wpilibj.AnalogInput;
 import edu.wpi.first.wpilibj.RobotController;
 import java.util.Queue;
+import java.util.concurrent.Executor;
+import java.util.concurrent.Executors;
 import java.util.function.Supplier;
 
 public class ModuleIOKrakenFOC implements ModuleIO {
@@ -52,6 +54,7 @@ public class ModuleIOKrakenFOC implements ModuleIO {
   // Controller Configs
   private final TalonFXConfiguration driveTalonConfig = new TalonFXConfiguration();
   private final TalonFXConfiguration turnTalonConfig = new TalonFXConfiguration();
+  private static final Executor brakeModeExecutor = Executors.newFixedThreadPool(8);
 
   // Control
   private final VoltageOut voltageControl = new VoltageOut(0).withUpdateFreqHz(0);
@@ -227,16 +230,26 @@ public void setTurnPID(double kP, double kI, double kD) {
 
   @Override
   public void setDriveBrakeMode(boolean enable) {
-    driveTalonConfig.MotorOutput.NeutralMode =
-        enable ? NeutralModeValue.Brake : NeutralModeValue.Coast;
-    driveTalon.getConfigurator().apply(driveTalonConfig);
+    brakeModeExecutor.execute(
+        () -> {
+          synchronized (driveTalonConfig) {
+            driveTalonConfig.MotorOutput.NeutralMode =
+                enable ? NeutralModeValue.Brake : NeutralModeValue.Coast;
+            driveTalon.getConfigurator().apply(driveTalonConfig, 0.25);
+          }
+        });
   }
 
   @Override
   public void setTurnBrakeMode(boolean enable) {
-    turnTalonConfig.MotorOutput.NeutralMode =
-        enable ? NeutralModeValue.Brake : NeutralModeValue.Coast;
-    turnTalon.getConfigurator().apply(turnTalonConfig);
+    brakeModeExecutor.execute(
+        () -> {
+          synchronized (turnTalonConfig) {
+            turnTalonConfig.MotorOutput.NeutralMode =
+                enable ? NeutralModeValue.Brake : NeutralModeValue.Coast;
+            turnTalon.getConfigurator().apply(turnTalonConfig, 0.25);
+          }
+        });
   }
 
   @Override

src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSim.java

@@ -11,11 +11,13 @@
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.filter.SlewRateLimiter;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.simulation.DCMotorSim;
 import org.littletonrobotics.frc2024.Constants;
+import org.littletonrobotics.frc2024.subsystems.drive.DriveConstants.ModuleConfig;
 
 public class ModuleIOSim implements ModuleIO {
   private final DCMotorSim driveSim =
@@ -32,6 +34,9 @@ public class ModuleIOSim implements ModuleIO {
   private double turnAppliedVolts = 0.0;
   private final Rotation2d turnAbsoluteInitPosition;
 
+  private boolean driveCoast = false;
+  private SlewRateLimiter driveVoltsLimiter = new SlewRateLimiter(4.0);
+
   public ModuleIOSim(ModuleConfig config) {
     turnAbsoluteInitPosition = config.absoluteEncoderOffset();
     turnFeedback.enableContinuousInput(-Math.PI, Math.PI);
@@ -43,6 +48,12 @@ public void updateInputs(ModuleIOInputs inputs) {
       stop();
     }
 
+    if (driveCoast && DriverStation.isDisabled()) {
+      runDriveVolts(driveVoltsLimiter.calculate(driveAppliedVolts));
+    } else {
+      driveVoltsLimiter.reset(driveAppliedVolts);
+    }
+
     driveSim.update(Constants.loopPeriodSecs);
     turnSim.update(Constants.loopPeriodSecs);
 
@@ -101,6 +112,11 @@ public void setTurnPID(double kP, double kI, double kD) {
     turnFeedback.setPID(kP, kI, kD);
   }
 
+  @Override
+  public void setDriveBrakeMode(boolean enable) {
+    driveCoast = !enable;
+  }
+
   @Override
   public void stop() {
     runDriveVolts(0.0);