Set turn motor position to absolute position at start up

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

@@ -20,6 +20,7 @@
 import java.util.concurrent.locks.Lock;
 import java.util.concurrent.locks.ReentrantLock;
 import java.util.stream.IntStream;
+import lombok.Getter;
 import lombok.experimental.ExtensionMethod;
 import org.littletonrobotics.frc2024.Constants;
 import org.littletonrobotics.frc2024.RobotState;
@@ -39,11 +40,10 @@
 
 @ExtensionMethod({GeomUtil.class})
 public class Drive extends SubsystemBase {
-  private static final LoggedTunableNumber coastSpeedLimit =
-      new LoggedTunableNumber(
-          "Drive/CoastSpeedLimit", DriveConstants.driveConfig.maxLinearVelocity() * 0.6);
-  private static final LoggedTunableNumber coastDisableTime =
-      new LoggedTunableNumber("Drive/CoastDisableTimeSeconds", 0.5);
+  private static final LoggedTunableNumber coastWaitTime =
+      new LoggedTunableNumber("Drive/CoastWaitTimeSeconds", 0.5);
+  private static final LoggedTunableNumber coastMetersPerSecThreshold =
+      new LoggedTunableNumber("Drive/CoastMetersPerSecThreshold", 0.05);
 
   public enum DriveMode {
     /** Driving with input from driver joysticks. (Default) */
@@ -65,7 +65,6 @@ public static class OdometryTimestampInputs {
   }
 
   public static final Lock odometryLock = new ReentrantLock();
-  // TODO: DO THIS BETTER!
   public static final Queue<Double> timestampQueue = new ArrayBlockingQueue<>(100);
 
   private final OdometryTimestampInputsAutoLogged odometryTimestampInputs =
@@ -74,15 +73,19 @@ public static class OdometryTimestampInputs {
   private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged();
   private final Module[] modules = new Module[4];
 
+  // Store previous positions and time for filtering odometry data
   private SwerveDriveWheelPositions lastPositions = null;
   private double lastTime = 0.0;
 
   /** Active drive mode. */
   private DriveMode currentDriveMode = DriveMode.TELEOP;
 
-  private double characterizationVolts = 0.0;
+  private double characterizationInput = 0.0;
   private boolean modulesOrienting = false;
-  private final Timer coastTimer = new Timer();
+  private final Timer lastMovementTimer = new Timer();
+
+  @Getter
+  @AutoLogOutput(key = "Drive/BrakeModeEnabled")
   private boolean brakeModeEnabled = true;
 
   private ChassisSpeeds desiredSpeeds = new ChassisSpeeds();
@@ -96,7 +99,7 @@ public static class OdometryTimestampInputs {
             new SwerveModuleState(),
             new SwerveModuleState()
           });
-  private SwerveSetpointGenerator setpointGenerator;
+  private final SwerveSetpointGenerator setpointGenerator;
 
   private final TeleopDriveController teleopDriveController;
   private TrajectoryController trajectoryController = null;
@@ -109,6 +112,8 @@ public Drive(GyroIO gyroIO, ModuleIO fl, ModuleIO fr, ModuleIO bl, ModuleIO br)
     modules[1] = new Module(fr, 1);
     modules[2] = new Module(bl, 2);
     modules[3] = new Module(br, 3);
+    lastMovementTimer.start();
+    setBrakeMode(true);
 
     setpointGenerator =
         SwerveSetpointGenerator.builder()
@@ -151,7 +156,6 @@ public void periodic() {
     }
     // Pass odometry data to robot state
     for (int i = 0; i < minOdometryUpdates; i++) {
-      boolean includeMeasurement = true;
       int odometryIndex = i;
       Rotation2d yaw = gyroInputs.connected ? gyroInputs.odometryYawPositions[i] : null;
       // Get all four swerve module positions at that odometry update
@@ -161,64 +165,55 @@ public void periodic() {
               Arrays.stream(modules)
                   .map(module -> module.getModulePositions()[odometryIndex])
                   .toArray(SwerveModulePosition[]::new));
-      // Filtering
+      // Filtering based on delta wheel positions
+      boolean includeMeasurement = true;
       if (lastPositions != null) {
-        double dt = Timer.getFPGATimestamp() - lastTime;
-        for (int j = 0; j < 4; j++) {
+        double dt = odometryTimestampInputs.timestamps[i] - lastTime;
+        for (int j = 0; j < modules.length; j++) {
           double velocity =
               (wheelPositions.positions[j].distanceMeters
                       - lastPositions.positions[j].distanceMeters)
                   / dt;
           double omega =
               wheelPositions.positions[j].angle.minus(lastPositions.positions[j].angle).getRadians()
                   / dt;
-
-          if (Math.abs(omega) > currentModuleLimits.maxSteeringVelocity() * 100.0
-              || Math.abs(velocity) > currentModuleLimits.maxDriveVelocity() * 100.0) {
+          // Check if delta is too large
+          if (Math.abs(omega) > currentModuleLimits.maxSteeringVelocity() * 5.0
+              || Math.abs(velocity) > currentModuleLimits.maxDriveVelocity() * 5.0) {
             includeMeasurement = false;
             break;
           }
         }
       }
+      // If delta isn't too large we can include the measurement.
       if (includeMeasurement) {
         lastPositions = wheelPositions;
         RobotState.getInstance()
             .addOdometryObservation(
                 new RobotState.OdometryObservation(
                     wheelPositions, yaw, odometryTimestampInputs.timestamps[i]));
+        lastTime = odometryTimestampInputs.timestamps[i];
       }
     }
-    lastTime = Timer.getFPGATimestamp();
 
-    // update current velocities use gyro when possible
+    // Update current velocities use gyro when possible
     ChassisSpeeds robotRelativeVelocity = getSpeeds();
     robotRelativeVelocity.omegaRadiansPerSecond =
         gyroInputs.connected
             ? gyroInputs.yawVelocityRadPerSec
             : robotRelativeVelocity.omegaRadiansPerSecond;
     RobotState.getInstance().addVelocityData(robotRelativeVelocity.toTwist2d());
 
-    // Disabled, stop modules and coast
-    if (DriverStation.isDisabled()) {
-      Arrays.stream(modules).forEach(Module::stop);
-      if (Math.hypot(
-                  robotRelativeVelocity.vxMetersPerSecond, robotRelativeVelocity.vyMetersPerSecond)
-              <= coastSpeedLimit.get()
-          && brakeModeEnabled) {
-        setBrakeMode(false);
-        coastTimer.stop();
-        coastTimer.reset();
-      } else if (coastTimer.hasElapsed(coastDisableTime.get()) && brakeModeEnabled) {
-        setBrakeMode(false);
-        coastTimer.stop();
-        coastTimer.reset();
-      } else {
-        coastTimer.start();
-      }
-      return;
-    } else {
-      // Brake mode
-      setBrakeMode(true);
+    // Update brake mode
+    // Reset movement timer if moved
+    if (Arrays.stream(modules)
+        .anyMatch(module -> module.getVelocityMetersPerSec() > coastMetersPerSecThreshold.get())) {
+      lastMovementTimer.reset();
+    }
+    if (DriverStation.isEnabled()) {
+      setBrakeMode(true); // Always in brake mode during teleop
+    } else if (lastMovementTimer.hasElapsed(coastWaitTime.get())) {
+      setBrakeMode(false);
     }
 
     // Run drive based on current mode
@@ -246,7 +241,7 @@ public void periodic() {
       case CHARACTERIZATION -> {
         // run characterization
         for (Module module : modules) {
-          module.runCharacterization(characterizationVolts);
+          module.runCharacterization(characterizationInput);
         }
       }
       default -> {}
@@ -344,7 +339,7 @@ public boolean isAutoAimGoalCompleted() {
   /** Runs forwards at the commanded voltage. */
   public void runCharacterizationVolts(double volts) {
     currentDriveMode = DriveMode.CHARACTERIZATION;
-    characterizationVolts = volts;
+    characterizationInput = volts;
   }
 
   /** Disables the characterization mode. */
@@ -361,10 +356,12 @@ public double getCharacterizationVelocity() {
     return driveVelocityAverage / 4.0;
   }
 
-  /** Set brake mode enabled */
+  /** Set brake mode to {@code enabled} doesn't change brake mode if already set */
   public void setBrakeMode(boolean enabled) {
+    if (brakeModeEnabled != enabled) {
+      Arrays.stream(modules).forEach(module -> module.setBrakeMode(enabled));
+    }
     brakeModeEnabled = enabled;
-    Arrays.stream(modules).forEach(module -> module.setBrakeMode(enabled));
   }
 
   public Command orientModules(Rotation2d orientation) {

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

@@ -20,15 +20,6 @@
 
 /** All Constants Measured in Meters and Radians (m/s, m/s^2, rad/s, rad/s^2) */
 public final class DriveConstants {
-  // For Kraken
-  public static class KrakenDriveConstants {
-    public static final boolean useTorqueCurrentFOC = false;
-    public static final boolean useMotionMagic = false;
-    public static final double motionMagicCruiseVelocity = 0.0;
-    public static final double motionMagicAcceleration = 0.0;
-  }
-
-  // Drive Constants
   public static DriveConfig driveConfig =
       switch (Constants.getRobot()) {
         case SIMBOT, COMPBOT ->

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

@@ -15,6 +15,7 @@
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import lombok.Getter;
+import org.littletonrobotics.frc2024.util.Alert;
 import org.littletonrobotics.frc2024.util.LoggedTunableNumber;
 import org.littletonrobotics.junction.Logger;
 
@@ -31,62 +32,83 @@ public class Module {
       new LoggedTunableNumber("Drive/Module/TurnkP", moduleConstants.turnkP());
   private static final LoggedTunableNumber turnkD =
       new LoggedTunableNumber("Drive/Module/TurnkD", moduleConstants.turnkD());
+  private static final String[] moduleNames = new String[] {"FL", "FR", "BL", "BR"};
 
   private final int index;
   private final ModuleIO io;
   private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged();
-  private SimpleMotorFeedforward driveFF =
+  private SimpleMotorFeedforward ff =
       new SimpleMotorFeedforward(moduleConstants.ffkS(), moduleConstants.ffkV(), 0.0);
-
   @Getter private SwerveModuleState setpointState = new SwerveModuleState();
 
+  // Alerts
+  private final Alert driveMotorDisconnected;
+  private final Alert turnMotorDisconnected;
+
   public Module(ModuleIO io, int index) {
     this.io = io;
     this.index = index;
+
+    driveMotorDisconnected =
+        new Alert(
+            "Drive", moduleNames[index] + " drive motor disconnected!", Alert.AlertType.WARNING);
+    turnMotorDisconnected =
+        new Alert(
+            "Drive", moduleNames[index] + " turn motor disconnected!", Alert.AlertType.WARNING);
   }
 
   /** Called while blocking odometry thread */
   public void updateInputs() {
     io.updateInputs(inputs);
-    Logger.processInputs("Drive/Module" + index, inputs);
+    Logger.processInputs("Drive/" + moduleNames[index] + "_Module", inputs);
 
-    // Update FF and controllers
+    // Update ff and controllers
     LoggedTunableNumber.ifChanged(
         hashCode(),
-        () -> {
-          driveFF = new SimpleMotorFeedforward(drivekS.get(), drivekV.get(), 0);
-          io.setDriveFF(drivekS.get(), drivekV.get(), 0);
-        },
+        () -> ff = new SimpleMotorFeedforward(drivekS.get(), drivekV.get(), 0),
         drivekS,
         drivekV);
     LoggedTunableNumber.ifChanged(
         hashCode(), () -> io.setDrivePID(drivekP.get(), 0, drivekD.get()), drivekP, drivekD);
     LoggedTunableNumber.ifChanged(
         hashCode(), () -> io.setTurnPID(turnkP.get(), 0, turnkD.get()), turnkP, turnkD);
+
+    // Display alerts
+    driveMotorDisconnected.set(!inputs.driveMotorConnected);
+    turnMotorDisconnected.set(!inputs.turnMotorConnected);
   }
 
+  /** Runs to {@link SwerveModuleState} */
   public void runSetpoint(SwerveModuleState setpoint) {
     setpointState = setpoint;
-    io.setDriveVelocitySetpoint(
+    io.runDriveVelocitySetpoint(
         setpoint.speedMetersPerSecond / driveConfig.wheelRadius(),
-        driveFF.calculate(setpoint.speedMetersPerSecond / driveConfig.wheelRadius()));
-    io.setTurnPositionSetpoint(setpoint.angle.getRadians());
+        ff.calculate(setpoint.speedMetersPerSecond / driveConfig.wheelRadius()));
+    io.runTurnPositionSetpoint(setpoint.angle.getRadians());
   }
 
-  public void runCharacterization(double volts) {
-    io.setTurnPositionSetpoint(0.0);
-    io.setDriveVoltage(volts);
+  /** Runs characterization volts or amps depending on using voltage or current control. */
+  public void runCharacterization(double input) {
+    io.runTurnPositionSetpoint(0.0);
+    if (inputs.hasCurrentControl) {
+      io.runDriveCurrent(input);
+    } else {
+      io.runDriveVolts(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();
   }
 
+  /** Get all latest {@link SwerveModulePosition}'s from last cycle. */
   public SwerveModulePosition[] getModulePositions() {
     int minOdometryPositions =
         Math.min(inputs.odometryDrivePositionsMeters.length, inputs.odometryTurnPositions.length);
@@ -99,26 +121,32 @@ public SwerveModulePosition[] getModulePositions() {
     return positions;
   }
 
+  /** Get turn angle of module as {@link Rotation2d}. */
   public Rotation2d getAngle() {
     return inputs.turnAbsolutePosition;
   }
 
+  /** Get position of wheel in meters. */
   public double getPositionMeters() {
     return inputs.drivePositionRad * driveConfig.wheelRadius();
   }
 
+  /** Get velocity of wheel in m/s. */
   public double getVelocityMetersPerSec() {
     return inputs.driveVelocityRadPerSec * driveConfig.wheelRadius();
   }
 
+  /** Get current {@link SwerveModulePosition} of module. */
   public SwerveModulePosition getPosition() {
     return new SwerveModulePosition(getPositionMeters(), getAngle());
   }
 
+  /** Get current {@link SwerveModuleState} of module. */
   public SwerveModuleState getState() {
     return new SwerveModuleState(getVelocityMetersPerSec(), getAngle());
   }
 
+  /** Get velocity of drive wheel for characterization */
   public double getCharacterizationVelocity() {
     return inputs.driveVelocityRadPerSec;
   }