Use ctre sim for swerve

Signed-off-by: Jade Turner <[email protected]>

Author: spacey-sooty

src/main/java/org/curtinfrc/frc2025/Constants.java

@@ -15,7 +15,7 @@
  * (log replay from a file).
  */
 public final class Constants {
-  public static final RobotType robotType = RobotType.COMPBOT;
+  public static final RobotType robotType = RobotType.SIMBOT;
   public static final double ROBOT_X = 0.705;
   public static final double ROBOT_Y = 0.730;
   public static final double FIELD_LENGTH = aprilTagLayout.getFieldLength();

src/main/java/org/curtinfrc/frc2025/generated/CompTunerConstants.java

@@ -94,11 +94,11 @@ public class CompTunerConstants {
   private static final int kPigeonId = 0;
 
   // These are only used for simulation
-  private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01);
-  private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01);
+  private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(1);
+  private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.1);
   // Simulated voltage necessary to overcome friction
   private static final Voltage kSteerFrictionVoltage = Volts.of(0.2);
-  private static final Voltage kDriveFrictionVoltage = Volts.of(0.2);
+  private static final Voltage kDriveFrictionVoltage = Volts.of(0.4);
 
   public static final SwerveDrivetrainConstants DrivetrainConstants =
       new SwerveDrivetrainConstants()

src/main/java/org/curtinfrc/frc2025/subsystems/drive/ModuleIOSim.java

@@ -1,138 +1,67 @@
-// Copyright 2021-2024 FRC 6328
-// http://github.com/Mechanical-Advantage
-//
-// This program is free software; you can redistribute it and/or
-// modify it under the terms of the GNU General Public License
-// version 3 as published by the Free Software Foundation or
-// available in the root directory of this project.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU General Public License for more details.
-
 package org.curtinfrc.frc2025.subsystems.drive;
 
 import com.ctre.phoenix6.swerve.SwerveModuleConstants;
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.system.plant.LinearSystemId;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.simulation.DCMotorSim;
 
-/**
- * Physics sim implementation of module IO. The sim models are configured using a set of module
- * constants from Phoenix. Simulation is always based on voltage control.
- */
-public class ModuleIOSim implements ModuleIO {
-  // TunerConstants doesn't support separate sim constants, so they are declared locally
-  private static final double DRIVE_KP = 0.05;
-  private static final double DRIVE_KD = 0.0;
-  private static final double DRIVE_KS = 0.0;
-  private static final double DRIVE_KV_ROT =
-      0.91035; // Same units as TunerConstants: (volt * secs) / rotation
-  private static final double DRIVE_KV = 1.0 / Units.rotationsToRadians(1.0 / DRIVE_KV_ROT);
-  private static final double TURN_KP = 8.0;
-  private static final double TURN_KD = 0.0;
+public class ModuleIOSim extends ModuleIOTalonFX {
   private static final DCMotor DRIVE_GEARBOX = DCMotor.getKrakenX60Foc(1);
-  private static final DCMotor TURN_GEARBOX = DCMotor.getKrakenX60Foc(1);
+  private static final DCMotor TURN_GEARBOX = DCMotor.getKrakenX60(1);
 
   private final DCMotorSim driveSim;
-  private final DCMotorSim turnSim;
-
-  private boolean driveClosedLoop = false;
-  private boolean turnClosedLoop = false;
-  private PIDController driveController = new PIDController(DRIVE_KP, 0, DRIVE_KD);
-  private PIDController turnController = new PIDController(TURN_KP, 0, TURN_KD);
-  private double driveFFVolts = 0.0;
-  private double driveAppliedVolts = 0.0;
-  private double turnAppliedVolts = 0.0;
+  private final DCMotorSim steerSim;
 
   public ModuleIOSim(SwerveModuleConstants constants) {
-    // Create drive and turn sim models
+    super(constants);
+    // Create drive and steer sim models
     driveSim =
         new DCMotorSim(
             LinearSystemId.createDCMotorSystem(
                 DRIVE_GEARBOX, constants.DriveInertia, constants.DriveMotorGearRatio),
             DRIVE_GEARBOX);
-    turnSim =
+    steerSim =
         new DCMotorSim(
             LinearSystemId.createDCMotorSystem(
                 TURN_GEARBOX, constants.SteerInertia, constants.SteerMotorGearRatio),
             TURN_GEARBOX);
-
-    // Enable wrapping for turn PID
-    turnController.enableContinuousInput(-Math.PI, Math.PI);
   }
 
-  @Override
-  public void updateInputs(ModuleIOInputs inputs) {
-    // Run closed-loop control
-    if (driveClosedLoop) {
-      driveAppliedVolts =
-          driveFFVolts + driveController.calculate(driveSim.getAngularVelocityRadPerSec());
-    } else {
-      driveController.reset();
-    }
-    if (turnClosedLoop) {
-      turnAppliedVolts = turnController.calculate(turnSim.getAngularPositionRad());
-    } else {
-      turnController.reset();
-    }
-
-    // Update simulation state
-    driveSim.setInputVoltage(MathUtil.clamp(driveAppliedVolts, -12.0, 12.0));
-    turnSim.setInputVoltage(MathUtil.clamp(turnAppliedVolts, -12.0, 12.0));
-    driveSim.update(0.02);
-    turnSim.update(0.02);
-
-    // Update drive inputs
-    inputs.driveConnected = true;
-    inputs.drivePositionRad = driveSim.getAngularPositionRad();
-    inputs.driveVelocityRadPerSec = driveSim.getAngularVelocityRadPerSec();
-    inputs.driveAppliedVolts = driveAppliedVolts;
-    inputs.driveCurrentAmps = Math.abs(driveSim.getCurrentDrawAmps());
-
-    // Update turn inputs
-    inputs.turnConnected = true;
-    inputs.turnEncoderConnected = true;
-    inputs.turnAbsolutePosition = new Rotation2d(turnSim.getAngularPositionRad());
-    inputs.turnPosition = new Rotation2d(turnSim.getAngularPositionRad());
-    inputs.turnVelocityRadPerSec = turnSim.getAngularVelocityRadPerSec();
-    inputs.turnAppliedVolts = turnAppliedVolts;
-    inputs.turnCurrentAmps = Math.abs(turnSim.getCurrentDrawAmps());
+  private double driveOutput(double raw) {
+    return raw * constants.DriveMotorGearRatio;
+  }
 
-    // Update odometry inputs (50Hz because high-frequency odometry in sim doesn't matter)
-    inputs.odometryTimestamps = new double[] {Timer.getFPGATimestamp()};
-    inputs.odometryDrivePositionsRad = new double[] {inputs.drivePositionRad};
-    inputs.odometryTurnPositions = new Rotation2d[] {inputs.turnPosition};
+  private double driveVoltage(double raw) {
+    return (Math.abs(raw) > constants.DriveFrictionVoltage) ? raw : 0;
   }
 
-  @Override
-  public void setDriveOpenLoop(double output) {
-    driveClosedLoop = false;
-    driveAppliedVolts = output;
+  private double steerOutput(double raw) {
+    return (constants.SteerMotorInverted ? -raw : raw) * constants.SteerMotorGearRatio;
   }
 
-  @Override
-  public void setTurnOpenLoop(double output) {
-    turnClosedLoop = false;
-    turnAppliedVolts = output;
+  private double steerVoltage(double raw) {
+    return (Math.abs(raw) > constants.SteerFrictionVoltage) ? raw : 0;
   }
 
-  @Override
-  public void setDriveVelocity(double velocityRadPerSec, double feedforward) {
-    driveClosedLoop = true;
-    driveFFVolts = DRIVE_KS * Math.signum(velocityRadPerSec) + DRIVE_KV * velocityRadPerSec;
-    driveController.setSetpoint(velocityRadPerSec);
+  private void updateSimState(double dt) {
+    var driveSimState = driveTalon.getSimState();
+    driveSim.setInputVoltage(driveVoltage(driveSimState.getMotorVoltage()));
+    driveSim.update(dt);
+    driveSimState.setRawRotorPosition(driveOutput(driveSim.getAngularPositionRotations()));
+    driveSimState.setRotorVelocity(driveOutput(driveSim.getAngularVelocityRPM() / 60));
+
+    var steerSimState = turnTalon.getSimState();
+    var encoderSimState = cancoder.getSimState();
+    steerSim.setInputVoltage(steerVoltage(steerSimState.getMotorVoltage()));
+    steerSim.update(dt);
+    encoderSimState.setRawPosition(-steerSim.getAngularPositionRotations());
+    steerSimState.setRawRotorPosition(steerOutput(steerSim.getAngularPositionRotations()));
+    steerSimState.setRotorVelocity(steerOutput(steerSim.getAngularVelocityRPM() / 60));
   }
 
   @Override
-  public void setTurnPosition(Rotation2d rotation) {
-    turnClosedLoop = true;
-    turnController.setSetpoint(rotation.getRadians());
+  public void updateInputs(ModuleIOInputs inputs) {
+    super.updateInputs(inputs);
+    updateSimState(0.02);
   }
 }

src/main/java/org/curtinfrc/frc2025/subsystems/drive/ModuleIOTalonFX.java

@@ -39,14 +39,14 @@
  * <p>Device configuration and other behaviors not exposed by TunerConstants can be customized here.
  */
 public class ModuleIOTalonFX implements ModuleIO {
-  private final SwerveModuleConstants<
+  protected final SwerveModuleConstants<
           TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
       constants;
 
   // Hardware objects
-  private final TalonFX driveTalon;
-  private final TalonFX turnTalon;
-  private final CANcoder cancoder;
+  protected final TalonFX driveTalon;
+  protected final TalonFX turnTalon;
+  protected final CANcoder cancoder;
 
   // Voltage control requests
   private final VoltageOut voltageRequest = new VoltageOut(0).withEnableFOC(true);