Set turn motor position to absolute position at start up

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) {}