Refactor module ios

src/main/java/frc/robot/subsystems/swerve/module/Module.java

@@ -15,7 +15,7 @@ public class Module {
   public record ModuleConstants(
       int id, String prefix, int driveID, int turnID, int cancoderID, Rotation2d cancoderOffset) {}
 
-  private final ModuleIO io;
+  private final ModuleIOReal io;
   private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged();
   private final ModuleConstants constants;
 
@@ -32,7 +32,7 @@ public record ModuleConstants(
   private final Debouncer turnEncoderConnectedDebouncer =
       new Debouncer(0.5, Debouncer.DebounceType.kFalling);
 
-  public Module(ModuleIO io) {
+  public Module(ModuleIOReal io) {
     this.io = io;
     this.constants = io.getModuleConstants();
 

src/main/java/frc/robot/subsystems/swerve/module/ModuleIOReal.java

@@ -19,15 +19,43 @@
 import frc.robot.subsystems.swerve.odometry.PhoenixOdometryThread.Registration;
 import frc.robot.subsystems.swerve.odometry.PhoenixOdometryThread.SignalType;
 import java.util.Optional;
+import org.littletonrobotics.junction.AutoLog;
 import org.littletonrobotics.junction.Logger;
 
-public class ModuleIOReal implements ModuleIO {
+public class ModuleIOReal {
+
+  @AutoLog
+  public static class ModuleIOInputs {
+    // For the drive motor
+    // The conversions from rotations to meters are done in the SensorToMechanismRatio config for
+    // the motors
+    public boolean driveConnected = false;
+    public double driveVelocityMetersPerSec = 0.0;
+    public double drivePositionMeters = 0.0;
+    public double driveAppliedVolts = 0.0;
+    public double driveTempC = 0.0;
+    public double driveStatorCurrentAmps = 0.0;
+    public double driveSupplyCurrentAmps = 0.0;
+
+    // For the turn motor
+    public boolean turnConnected = false;
+    public Rotation2d turnPosition = new Rotation2d();
+    public double turnVelocityRadPerSec = 0.0;
+    public double turnAppliedVolts = 0.0;
+    public double turnTempC = 0.0;
+    public double turnStatorCurrentAmps = 0.0;
+    public double turnSupplyCurrentAmps = 0.0;
+
+    public boolean cancoderConnected = false;
+    public Rotation2d cancoderAbsolutePosition = new Rotation2d();
+  }
+
   private final ModuleConstants constants;
 
   // Hardware
-  private final TalonFX driveTalon;
-  private final TalonFX turnTalon;
-  private final CANcoder cancoder;
+  protected final TalonFX driveTalon;
+  protected final TalonFX turnTalon;
+  protected final CANcoder cancoder;
 
   // Status signals
   // For drive
@@ -124,7 +152,6 @@ public ModuleIOReal(ModuleConstants moduleConstants, CANBus canbus) {
     cancoder.optimizeBusUtilization();
   }
 
-  @Override
   public void updateInputs(ModuleIOInputs inputs) {
     Logger.recordOutput(
         "Module" + constants.prefix() + "refresh status code",
@@ -178,27 +205,26 @@ public void updateInputs(ModuleIOInputs inputs) {
         Rotation2d.fromRotations(cancoderAbsolutePosition.getValueAsDouble());
   }
 
-  @Override
   public void setDriveVoltage(double volts, boolean withFoc) {
     driveTalon.setControl(driveVoltage.withOutput(volts).withEnableFOC(withFoc));
   }
 
-  @Override
+  public void setDriveVoltage(double volts) {
+    setDriveVoltage(volts, true);
+  }
+
   public void setDriveVelocitySetpoint(double setpointMetersPerSecond) {
     driveTalon.setControl(driveVelocityControl.withVelocity(setpointMetersPerSecond));
   }
 
-  @Override
   public void setTurnVoltage(double volts) {
     turnTalon.setControl(turnVoltage.withOutput(volts));
   }
 
-  @Override
   public void setTurnPositionSetpoint(Rotation2d setpoint) {
     turnTalon.setControl(turnPID.withPosition(setpoint.getRotations()));
   }
 
-  @Override
   public ModuleConstants getModuleConstants() {
     return constants;
   }

src/main/java/frc/robot/subsystems/swerve/module/ModuleIOSim.java

@@ -1,110 +1,20 @@
 package frc.robot.subsystems.swerve.module;
 
 import static edu.wpi.first.units.Units.Radian;
-import static edu.wpi.first.units.Units.RadiansPerSecond;
 
-import com.ctre.phoenix6.BaseStatusSignal;
 import com.ctre.phoenix6.CANBus;
-import com.ctre.phoenix6.StatusSignal;
-import com.ctre.phoenix6.controls.MotionMagicVoltage;
-import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC;
-import com.ctre.phoenix6.controls.VoltageOut;
-import com.ctre.phoenix6.hardware.CANcoder;
-import com.ctre.phoenix6.hardware.TalonFX;
-import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.units.measure.Angle;
-import edu.wpi.first.units.measure.AngularVelocity;
 import frc.robot.subsystems.swerve.SwerveSubsystem;
-import frc.robot.subsystems.swerve.module.Module.ModuleConstants;
 import frc.robot.utils.MaplePhoenixUtil;
 import org.ironmaple.simulation.drivesims.SwerveModuleSimulation;
 
-public class ModuleIOSim implements ModuleIO {
-
-  private final Module.ModuleConstants constants;
-
-  private final TalonFX driveTalon;
-  private final TalonFX turnTalon;
-  private final CANcoder cancoder;
-
-  // Status signals
-  private final BaseStatusSignal drivePosition;
-  private final BaseStatusSignal driveVelocity;
-  private final BaseStatusSignal driveTemp;
-  private final BaseStatusSignal driveSupplyCurrent;
-  private final BaseStatusSignal driveStatorCurrent;
-  private final BaseStatusSignal driveAppliedVolts;
-
-  private final BaseStatusSignal cancoderAbsolutePosition;
-  private final BaseStatusSignal turnPosition;
-  private final StatusSignal<AngularVelocity> turnVelocity;
-  private final BaseStatusSignal turnTemp;
-  private final BaseStatusSignal turnSupplyCurrent;
-  private final BaseStatusSignal turnStatorCurrent;
-  private final BaseStatusSignal turnAppliedVolts;
-
-  // Control modes
-  private final VoltageOut driveVoltage = new VoltageOut(0.0).withEnableFOC(true);
-  private final VoltageOut turnVoltage = new VoltageOut(0.0).withEnableFOC(true);
-  private final VelocityTorqueCurrentFOC driveVelocityControl =
-      new VelocityTorqueCurrentFOC(0.0).withSlot(0);
-  private final MotionMagicVoltage turnPID = new MotionMagicVoltage(0.0);
+public class ModuleIOSim extends ModuleIOReal {
 
   private final SwerveModuleSimulation simulation;
 
   public ModuleIOSim(
       Module.ModuleConstants constants, SwerveModuleSimulation simulation, CANBus canbus) {
-    this.constants = constants;
-
-    // Initialize hardware
-    driveTalon = new TalonFX(constants.driveID(), canbus);
-    turnTalon = new TalonFX(constants.turnID(), canbus);
-    cancoder = new CANcoder(constants.cancoderID(), canbus);
-
-    // Configure hardware
-    driveTalon.getConfigurator().apply(SwerveSubsystem.SWERVE_CONSTANTS.getDriveConfig());
-    turnTalon
-        .getConfigurator()
-        .apply(SwerveSubsystem.SWERVE_CONSTANTS.getTurnConfig(constants.cancoderID()));
-
-    cancoder
-        .getConfigurator()
-        .apply(SwerveSubsystem.SWERVE_CONSTANTS.getCancoderConfig(constants.cancoderOffset()));
-
-    // Initialize status signals
-    drivePosition = driveTalon.getPosition();
-    driveVelocity = driveTalon.getVelocity();
-    driveTemp = driveTalon.getDeviceTemp();
-    driveSupplyCurrent = driveTalon.getSupplyCurrent();
-    driveStatorCurrent = driveTalon.getStatorCurrent();
-    driveAppliedVolts = driveTalon.getMotorVoltage();
-
-    cancoderAbsolutePosition = cancoder.getAbsolutePosition();
-    turnPosition = turnTalon.getPosition();
-    turnVelocity = turnTalon.getVelocity();
-    turnTemp = turnTalon.getDeviceTemp();
-    turnSupplyCurrent = turnTalon.getSupplyCurrent();
-    turnStatorCurrent = turnTalon.getStatorCurrent();
-    turnAppliedVolts = turnTalon.getMotorVoltage();
-
-    BaseStatusSignal.setUpdateFrequencyForAll(
-        50.0,
-        drivePosition,
-        driveVelocity,
-        driveTemp,
-        driveAppliedVolts,
-        driveStatorCurrent,
-        driveSupplyCurrent,
-        cancoderAbsolutePosition,
-        turnPosition,
-        turnVelocity,
-        turnAppliedVolts,
-        turnStatorCurrent,
-        turnSupplyCurrent);
-
-    driveTalon.optimizeBusUtilization();
-    turnTalon.optimizeBusUtilization();
-    cancoder.optimizeBusUtilization();
+    super(constants, canbus);
 
     // Setup this modules simulation (lowk the only difference between Sim and Real except for not
     // using Async odo)
@@ -119,81 +29,4 @@ public ModuleIOSim(
             false,
             Angle.ofBaseUnits(constants.cancoderOffset().getRadians(), Radian)));
   }
-
-  @Override
-  public void updateInputs(ModuleIOInputs inputs) {
-    BaseStatusSignal.refreshAll(
-        drivePosition,
-        driveVelocity,
-        driveAppliedVolts,
-        driveStatorCurrent,
-        driveSupplyCurrent,
-        driveTemp,
-        cancoderAbsolutePosition,
-        turnPosition,
-        turnVelocity,
-        turnAppliedVolts,
-        turnStatorCurrent,
-        turnSupplyCurrent,
-        turnTemp);
-
-    inputs.driveConnected =
-        BaseStatusSignal.isAllGood(
-            drivePosition,
-            driveVelocity,
-            driveAppliedVolts,
-            driveStatorCurrent,
-            driveSupplyCurrent,
-            driveTemp);
-    inputs.drivePositionMeters = drivePosition.getValueAsDouble();
-    inputs.driveVelocityMetersPerSec = driveVelocity.getValueAsDouble();
-    inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble();
-    inputs.driveTempC = driveTemp.getValueAsDouble();
-    inputs.driveStatorCurrentAmps = driveStatorCurrent.getValueAsDouble();
-    inputs.driveSupplyCurrentAmps = driveSupplyCurrent.getValueAsDouble();
-
-    inputs.turnConnected =
-        BaseStatusSignal.isAllGood(
-            turnPosition,
-            turnVelocity,
-            turnAppliedVolts,
-            turnStatorCurrent,
-            turnSupplyCurrent,
-            turnTemp);
-    inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble());
-    inputs.turnVelocityRadPerSec = turnVelocity.getValue().in(RadiansPerSecond);
-    inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble();
-    inputs.turnTempC = turnTemp.getValueAsDouble();
-    inputs.turnStatorCurrentAmps = turnStatorCurrent.getValueAsDouble();
-    inputs.turnSupplyCurrentAmps = turnSupplyCurrent.getValueAsDouble();
-
-    inputs.cancoderConnected = BaseStatusSignal.isAllGood(cancoderAbsolutePosition);
-    inputs.cancoderAbsolutePosition =
-        Rotation2d.fromRotations(cancoderAbsolutePosition.getValueAsDouble());
-  }
-
-  @Override
-  public void setDriveVoltage(double volts, boolean withFoc) {
-    driveTalon.setControl(driveVoltage.withOutput(volts).withEnableFOC(withFoc));
-  }
-
-  @Override
-  public void setDriveVelocitySetpoint(double setpointMetersPerSecond) {
-    driveTalon.setControl(driveVelocityControl.withVelocity(setpointMetersPerSecond));
-  }
-
-  @Override
-  public void setTurnVoltage(double volts) {
-    turnTalon.setControl(turnVoltage.withOutput(volts));
-  }
-
-  @Override
-  public void setTurnPositionSetpoint(Rotation2d setpoint) {
-    turnTalon.setControl(turnPID.withPosition(setpoint.getRotations()));
-  }
-
-  @Override
-  public ModuleConstants getModuleConstants() {
-    return constants;
-  }
 }