Set turn motor position to absolute position at start up

Use pre-defined members for control rather than creating new control request each cycle
Use feedforward input from module always
Run drive and turn with amps
Make every control request one shot
Make alerts for drive and turn disconnected
Rename some methods in ModuleIO

src/main/java/org/littletonrobotics/frc2024/Constants.java

@@ -21,7 +21,7 @@
  */
 public final class Constants {
   public static final int loopPeriodMs = 20;
-  private static RobotType robotType = RobotType.DEVBOT;
+  private static RobotType robotType = RobotType.SIMBOT;
   public static final boolean tuningMode = true;
 
   public static RobotType getRobot() {

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

@@ -79,7 +79,7 @@ public static class OdometryTimestampInputs {
   /** 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 boolean brakeModeEnabled = true;
@@ -245,7 +245,7 @@ public void periodic() {
       case CHARACTERIZATION -> {
         // run characterization
         for (Module module : modules) {
-          module.runCharacterization(characterizationVolts);
+          module.runCharacterization(characterizationInput);
         }
       }
       default -> {}
@@ -343,7 +343,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. */

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 warnings
+    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;
   }

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

@@ -13,6 +13,10 @@
 public interface ModuleIO {
   @AutoLog
   class ModuleIOInputs {
+    public boolean driveMotorConnected = true;
+    public boolean turnMotorConnected = true;
+    public boolean hasCurrentControl = false;
+
     public double drivePositionRad = 0.0;
     public double driveVelocityRadPerSec = 0.0;
     public double driveAppliedVolts = 0.0;
@@ -34,26 +38,29 @@ class ModuleIOInputs {
   default void updateInputs(ModuleIOInputs inputs) {}
 
   /** Run drive motor at volts */
-  default void setDriveVoltage(double volts) {}
+  default void runDriveVolts(double volts) {}
 
   /** Run turn motor at volts */
-  default void setTurnVoltage(double volts) {}
+  default void runTurnVolts(double volts) {}
+
+  /** Run drive motor at current */
+  default void runDriveCurrent(double current) {}
 
-  /** Set drive velocity setpoint */
-  default void setDriveVelocitySetpoint(double velocityRadsPerSec, double ffVolts) {}
+  /** Run turn motor at current */
+  default void runTurnCurrent(double current) {}
 
-  /** Set turn position setpoint */
-  default void setTurnPositionSetpoint(double angleRads) {}
+  /** Run to drive velocity setpoint with feedforward */
+  default void runDriveVelocitySetpoint(double velocityRadsPerSec, double feedForward) {}
+
+  /** Run to turn position setpoint */
+  default void runTurnPositionSetpoint(double angleRads) {}
 
   /** Configure drive PID */
   default void setDrivePID(double kP, double kI, double kD) {}
 
   /** Configure turn PID */
   default void setTurnPID(double kP, double kI, double kD) {}
 
-  /** Configure drive feedforward for TorqueCurrentFOC */
-  default void setDriveFF(double kS, double kV, double kA) {}
-
   /** Enable or disable brake mode on the drive motor. */
   default void setDriveBrakeMode(boolean enable) {}