implemented ctre motor controller closed loops

src/main/java/frc/robot/RobotContainer.java

@@ -89,10 +89,10 @@ public RobotContainer() {
                 drive = new SwerveDrive(
                         SwerveDrive.DriveType.CTRE_ON_CANIVORE,
                         new GyroIOPigeon2(TunerConstants.DrivetrainConstants),
-                        new ModuleIOTalon(TunerConstants.DrivetrainConstants, TunerConstants.FrontLeft, "FrontLeft"),
-                        new ModuleIOTalon(TunerConstants.DrivetrainConstants, TunerConstants.FrontRight, "FrontRight"),
-                        new ModuleIOTalon(TunerConstants.DrivetrainConstants, TunerConstants.BackLeft, "BackLeft"),
-                        new ModuleIOTalon(TunerConstants.DrivetrainConstants, TunerConstants.BackRight, "BackRight"));
+                        new ModuleIOTalon(TunerConstants.FrontLeft, "FrontLeft"),
+                        new ModuleIOTalon(TunerConstants.FrontRight, "FrontRight"),
+                        new ModuleIOTalon(TunerConstants.BackLeft, "BackLeft"),
+                        new ModuleIOTalon(TunerConstants.BackRight, "BackRight"));
 
                 /* REV Chassis */
                 //                drive = new SwerveDrive(

src/main/java/frc/robot/subsystems/drive/IO/GyroIOPigeon2.java

@@ -37,7 +37,7 @@ public GyroIOPigeon2(int Pigeon2Id, String CANbusName, Pigeon2Configuration Pige
         yawVelocity = pigeon.getAngularVelocityZWorld();
 
         yawVelocity.setUpdateFrequency(100.0);
-        yawPositionInput = OdometryThread.registerSignalInput(pigeon.getYaw());
+        yawPositionInput = OdometryThread.registerSignalSignal(pigeon.getYaw());
 
         pigeon.optimizeBusUtilization();
     }

src/main/java/frc/robot/subsystems/drive/IO/ModuleIO.java

@@ -5,7 +5,11 @@
 
 package frc.robot.subsystems.drive.IO;
 
+import static edu.wpi.first.units.Units.Volts;
+
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.units.measure.Current;
+import edu.wpi.first.units.measure.Voltage;
 import org.littletonrobotics.junction.AutoLog;
 
 public interface ModuleIO {
@@ -24,27 +28,62 @@ class ModuleIOInputs {
         public double[] odometryDriveWheelRevolutions = new double[] {};
         public Rotation2d[] odometrySteerPositions = new Rotation2d[] {};
 
-        public boolean hardwareConnected = false;
+        public boolean driveMotorConnected = false;
+        public boolean steerMotorConnected = false;
+        public boolean steerEncoderConnected = false;
     }
 
     /** Updates the inputs */
     void updateInputs(ModuleIOInputs inputs);
 
     default void calibrate() {}
 
+    default void stop() {
+        requestDriveOpenLoop(Volts.zero());
+        requestSteerOpenLoop(Volts.zero());
+    }
+
+    /**
+     * Run the drive motor at a specified voltage open-loop control
+     *
+     * @param output the desired voltage output, from -12v to 12v
+     */
+    default void requestDriveOpenLoop(Voltage output) {}
+
+    /**
+     * Run the drive motor at a specified current open-loop control
+     *
+     * @param output the desired current output
+     */
+    default void requestDriveOpenLoop(Current output) {}
+
+    /**
+     * Run the steer motor at a specified voltage open-loop control
+     *
+     * @param output the desired voltage output, from -12v to 12v
+     */
+    default void requestSteerOpenLoop(Voltage output) {}
+
+    /**
+     * Run the steer motor at a specified current open-loop control
+     *
+     * @param output the desired current output
+     */
+    default void requestSteerOpenLoop(Current output) {}
+
     /**
-     * Run the drive motor at the specified percent speed.
+     * Runs a velocity close-loop control on the drive motor
      *
-     * @param speedPercent from -1 to 1, where 1 is the forward direction of the wheel
+     * @param desiredWheelVelocityRadPerSec the desired angular velocity of the wheel, in radians / second
      */
-    default void setDriveVoltage(double speedPercent) {}
+    default void requestDriveVelocityControl(double desiredWheelVelocityRadPerSec) {}
 
     /**
-     * Run the turn motor at the specified percent power.
+     * Runs a position close-loop control on the steer motor
      *
-     * @param powerPercent from -1 to 1, where 1 is counter-clockwise
+     * @param desiredSteerAbsoluteFacing the desired facing of the steer
      */
-    default void setSteerPowerPercent(double powerPercent) {}
+    default void requestSteerPositionControl(Rotation2d desiredSteerAbsoluteFacing) {}
 
     /** Enable or disable brake mode on the drive motor. */
     default void setDriveBrake(boolean enable) {}

src/main/java/frc/robot/subsystems/drive/IO/ModuleIOSim.java

@@ -7,8 +7,14 @@
 
 import static edu.wpi.first.units.Units.*;
 
+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.units.measure.Current;
+import edu.wpi.first.units.measure.Voltage;
 import frc.robot.constants.DriveTrainConstants;
 import java.util.Arrays;
+import org.ironmaple.simulation.SimulatedArena;
 import org.ironmaple.simulation.drivesims.SwerveModuleSimulation;
 import org.ironmaple.simulation.motorsims.SimulatedMotorController;
 
@@ -19,9 +25,23 @@
  * The flywheel sims are not physically accurate, but provide a decent approximation for the behavior of the module.
  */
 public class ModuleIOSim implements ModuleIO {
+    private static final double DRIVE_KS = 0.03;
+    private static final double DRIVE_KP = 0.05;
+    private static final double STEER_KP = 8.0;
+    private static final double STEER_KD = 0.0;
+
     private final SwerveModuleSimulation moduleSimulation;
     private final SimulatedMotorController.GenericMotorController driveMotor, steerMotor;
 
+    private boolean driveClosedLoopActivated = false;
+    private boolean steerClosedLoopActivated = false;
+    private final PIDController driveController;
+    private final PIDController steerController;
+    private double driveAppliedVolts = 0.0;
+    private double steerAppliedVolts = 0.0;
+    private double desiredWheelVelocityRadPerSec = 0.0;
+    private Rotation2d desiredSteerFacing = new Rotation2d();
+
     public ModuleIOSim(SwerveModuleSimulation moduleSimulation) {
         this.moduleSimulation = moduleSimulation;
         this.driveMotor = moduleSimulation
@@ -30,6 +50,13 @@ public ModuleIOSim(SwerveModuleSimulation moduleSimulation) {
         this.steerMotor = moduleSimulation
                 .useGenericControllerForSteer()
                 .withCurrentLimit(DriveTrainConstants.STEER_CURRENT_LIMIT);
+
+        this.driveController = new PIDController(DRIVE_KP, 0.0, 0.0);
+        this.steerController = new PIDController(STEER_KP, 0.0, STEER_KD);
+
+        // Enable wrapping for turn PID
+        steerController.enableContinuousInput(-Math.PI, Math.PI);
+        SimulatedArena.getInstance().addCustomSimulation((subTickNum) -> runControlLoops());
     }
 
     @Override
@@ -55,17 +82,73 @@ public void updateInputs(ModuleIOInputs inputs) {
                 .mapToDouble(angle -> angle.in(Revolutions))
                 .toArray();
         inputs.odometrySteerPositions = moduleSimulation.getCachedSteerAbsolutePositions();
+    }
+
+    public void runControlLoops() {
+        // Run control loops if activated
+        if (driveClosedLoopActivated) calculateDriveControlLoops();
+        else driveController.reset();
+        if (steerClosedLoopActivated) calculateSteerControlLoops();
+        else steerController.reset();
+
+        // Feed voltage to motor simulation
+        driveMotor.requestVoltage(Volts.of(driveAppliedVolts));
+        steerMotor.requestVoltage(Volts.of(steerAppliedVolts));
+    }
+
+    private void calculateDriveControlLoops() {
+        double ffVolts = moduleSimulation.driveMotorConfigs.motor.getVoltage(
+                        0, desiredWheelVelocityRadPerSec * moduleSimulation.DRIVE_GEAR_RATIO)
+                + Math.signum(desiredWheelVelocityRadPerSec) * DRIVE_KS;
+        double fbVolts = driveController.calculate(
+                moduleSimulation.getDriveWheelFinalSpeed().in(RadiansPerSecond), desiredWheelVelocityRadPerSec);
+        driveAppliedVolts = ffVolts + fbVolts;
+    }
+
+    private void calculateSteerControlLoops() {
+        steerAppliedVolts = steerController.calculate(
+                moduleSimulation.getSteerAbsoluteFacing().getRadians(), desiredSteerFacing.getRadians());
+    }
+
+    @Override
+    public void requestDriveOpenLoop(Voltage output) {
+        this.driveAppliedVolts = output.in(Volts);
+        this.driveClosedLoopActivated = false;
+    }
 
-        inputs.hardwareConnected = true;
+    @Override
+    public void requestSteerOpenLoop(Voltage output) {
+        this.steerAppliedVolts = output.in(Volts);
+        this.steerClosedLoopActivated = false;
+    }
+
+    @Override
+    public void requestDriveOpenLoop(Current output) {
+        DCMotor driveMotorModel = moduleSimulation.driveMotorConfigs.motor;
+        this.driveAppliedVolts = driveMotorModel.getVoltage(
+                driveMotorModel.getCurrent(output.in(Amps)),
+                moduleSimulation.getDriveEncoderUnGearedSpeed().in(RadiansPerSecond));
+        this.driveClosedLoopActivated = false;
+    }
+
+    @Override
+    public void requestSteerOpenLoop(Current output) {
+        DCMotor steerMotorModel = moduleSimulation.getSteerMotorConfigs().motor;
+        this.driveAppliedVolts = steerMotorModel.getVoltage(
+                steerMotorModel.getCurrent(output.in(Amps)),
+                moduleSimulation.getDriveEncoderUnGearedSpeed().in(RadiansPerSecond));
+        this.steerClosedLoopActivated = false;
     }
 
     @Override
-    public void setDriveVoltage(double volts) {
-        driveMotor.requestVoltage(Volts.of(volts));
+    public void requestDriveVelocityControl(double desiredWheelVelocityRadPerSec) {
+        this.desiredWheelVelocityRadPerSec = desiredWheelVelocityRadPerSec;
+        this.driveClosedLoopActivated = true;
     }
 
     @Override
-    public void setSteerPowerPercent(double powerPercent) {
-        steerMotor.requestVoltage(Volts.of(12).times(powerPercent));
+    public void requestSteerPositionControl(Rotation2d desiredSteerAbsoluteFacing) {
+        this.desiredSteerFacing = desiredSteerAbsoluteFacing;
+        this.steerClosedLoopActivated = true;
     }
 }