add simulated swerve

Author: amb2127

src/main/java/org/codeorange/frc2024/robot/Constants.java

@@ -1,7 +1,6 @@
 package org.codeorange.frc2024.robot;
 
 import edu.wpi.first.math.controller.ArmFeedforward;
-import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
 import edu.wpi.first.math.util.Units;
@@ -11,9 +10,7 @@
 import org.codeorange.frc2024.utility.swerve.SecondOrderKinematics;
 import org.jetbrains.annotations.NotNull;
 
-import java.io.File;
 import java.net.SocketException;
-import java.nio.file.Files;
 
 public final class Constants {
     //TODO: reorganize this mess
@@ -230,7 +227,7 @@ public static class Ports {
 
     public static final double freeSpeed = 95.81986264394831;
     public static final boolean USE_SECOND_ORDER_KINEMATICS = false;
-    public static final double STEER_MOTOR_POSITION_CONVERSION_FACTOR = 7.0 / 150.0;
+    public static final double STEER_MOTOR_RTS = 7.0 / 150.0;
     public static final double DRIVE_MOTOR_REDUCTION = 9 / 53.125;
 
     public static final double wheelBaseInches = isPrototype() ? 22.75 : 24.25; // not real number, just example

src/main/java/org/codeorange/frc2024/robot/Robot.java

@@ -7,6 +7,7 @@
 
 import com.choreo.lib.ChoreoTrajectory;
 import com.ctre.phoenix6.SignalLogger;
+import com.fasterxml.jackson.databind.Module;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -180,7 +181,7 @@ public void robotInit() {
                 Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables
             }
 
-            drive = new Drive(new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}, new GyroIO() {});
+            drive = new Drive(new ModuleIOSim(), new ModuleIOSim(), new ModuleIOSim(), new ModuleIOSim(), new GyroIO() {});
             wrist = new Wrist(new WristIO() {});
             elevator = new Elevator(new ElevatorIO() {});
             shooter = new Shooter(new ShooterIO(){});

src/main/java/org/codeorange/frc2024/subsystem/drive/ModuleIOSim.java

@@ -0,0 +1,81 @@
+package org.codeorange.frc2024.subsystem.drive;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.system.plant.DCMotor;
+import org.codeorange.frc2024.utility.logging.DCMotorSimAutoLogger;
+import org.codeorange.frc2024.utility.simulation.BetterDCMotorSim;
+import org.codeorange.frc2024.utility.wpimodified.PIDController;
+import org.littletonrobotics.junction.Logger;
+
+import static org.codeorange.frc2024.robot.Constants.*;
+
+public class ModuleIOSim implements ModuleIO {
+    private final PIDController driveController = new PIDController(SWERVE_DRIVE_GAINS.kP(), SWERVE_DRIVE_GAINS.kI(), SWERVE_DRIVE_GAINS.kD());
+    private final SimpleMotorFeedforward driveFeedforward = new SimpleMotorFeedforward(SWERVE_DRIVE_GAINS.kS(), SWERVE_DRIVE_GAINS.kV(), SWERVE_DRIVE_GAINS.kA());
+    private final PIDController steerController = new PIDController(SWERVE_STEER_GAINS.kP(), SWERVE_STEER_GAINS.kI(), SWERVE_STEER_GAINS.kD());
+
+    private final BetterDCMotorSim driveSim =
+            new BetterDCMotorSim(
+                    DCMotor.getKrakenX60Foc(1),
+                    1 / (DRIVE_MOTOR_REDUCTION * SWERVE_METER_PER_ROTATION),
+                    0.025,
+                    driveController,
+                    driveFeedforward);
+    private final BetterDCMotorSim steerSim =
+            new BetterDCMotorSim(
+                    DCMotor.getKrakenX60Foc(1),
+                    1 / STEER_MOTOR_RTS,
+                    0.004,
+                    steerController,
+                    new SimpleMotorFeedforward(0,0,0));
+    private final DCMotorSimAutoLogger driveLogger;
+    private final DCMotorSimAutoLogger steerLogger;
+
+    public ModuleIOSim() {
+        driveLogger = new DCMotorSimAutoLogger(driveSim);
+        steerLogger = new DCMotorSimAutoLogger(steerSim);
+    }
+
+    @Override
+    public void updateInputs(ModuleInputs inputs) {
+        inputs.driveMotor = driveLogger.log();
+        inputs.steerMotor = steerLogger.log();
+        inputs.steerMotorAbsolutePosition = inputs.steerMotor.position;
+
+        inputs.odometryTimestamps = new double[] {Logger.getRealTimestamp() * 1e-6};
+        inputs.odometryDrivePositionsMeters = new double[] {inputs.driveMotor.position};
+        inputs.odometryTurnPositions = new Rotation2d[] {Rotation2d.fromRotations(inputs.steerMotor.position)};
+    }
+
+    @Override
+    public void setDriveMotorVoltage(double volts) {
+        driveSim.setInputVoltage(volts);
+    }
+
+    @Override
+    public void setSteerMotorVoltage(double volts) {
+        steerSim.setInputVoltage(volts);
+    }
+
+    @Override
+    public void setSteerMotorPosition(double position) {
+        setSteerMotorPosition(position, 0);
+    }
+
+    @Override
+    public void setSteerMotorPosition(double position, double omega) {
+        steerSim.setPosition(position, omega * 0);
+    }
+
+    @Override
+    public void setDriveMotorVelocity(double velocity, double accel) {
+        driveSim.setVelocity(velocity);
+    }
+
+    @Override
+    public void setDriveMotorDutyCycle(double dutyCycle) {
+        setDriveMotorVoltage(12 * dutyCycle);
+    }
+}

src/main/java/org/codeorange/frc2024/subsystem/drive/ModuleIOTalonFX.java

@@ -102,7 +102,7 @@ public ModuleIOTalonFX(int id) {
         steerConfigs.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder;
         steerConfigs.Feedback.FeedbackRemoteSensorID = swerveCancoder.getDeviceID();
         steerConfigs.Feedback.SensorToMechanismRatio = 1;
-        steerConfigs.Feedback.RotorToSensorRatio = 1 / STEER_MOTOR_POSITION_CONVERSION_FACTOR;
+        steerConfigs.Feedback.RotorToSensorRatio = 1 / STEER_MOTOR_RTS;
         steerConfigs.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
         steerConfigs.MotorOutput.NeutralMode = NeutralModeValue.Coast;
         steerConfigs.MotionMagic.MotionMagicExpo_kV = 0;

src/main/java/org/codeorange/frc2024/utility/logging/DCMotorSimAutoLogger.java

@@ -17,7 +17,7 @@ public MotorInputs log() {
         inputs.CANID = -1;
         inputs.position = motorSim.getAngularPositionRotations();
         inputs.velocity = motorSim.getAngularVelocityRPM();
-        inputs.supplyVoltage = 12.5;
+        inputs.supplyVoltage = 12.0;
         inputs.motorVoltage = motorSim.getInputVoltage();
         inputs.supplyCurrent = motorSim.getCurrentDrawAmps();
         inputs.statorCurrent = motorSim.getCurrentDrawAmps() / (inputs.motorVoltage / 12.5);

src/main/java/org/codeorange/frc2024/utility/simulation/BetterDCMotorSim.java

@@ -1,17 +1,37 @@
 package org.codeorange.frc2024.utility.simulation;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N2;
 import edu.wpi.first.math.system.LinearSystem;
 import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.simulation.DCMotorSim;
+import org.codeorange.frc2024.utility.wpimodified.PIDController;
 
 /**
- * DCMotorSim but stores input voltage because i am lazy
+ * DCMotorSim but stores input voltage and has built in gain calculation because i am lazy
  */
 public class BetterDCMotorSim extends DCMotorSim {
     private double inputVoltage = 0;
+    private PIDController pidController = new PIDController(0, 0, 0);
+    private SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(0, 0, 0);
+
+    public void setPID(PIDController controller) {
+        pidController = controller;
+    }
+
+    public void setFeedforward(SimpleMotorFeedforward ff) {
+        feedforward = ff;
+    }
+
+    public void setGains(PIDController controller, SimpleMotorFeedforward ff) {
+        setPID(controller);
+        setFeedforward(ff);
+    }
 
     public BetterDCMotorSim(LinearSystem<N2, N1, N2> plant, DCMotor gearbox, double gearing) {
         super(plant, gearbox, gearing);
@@ -29,13 +49,41 @@ public BetterDCMotorSim(DCMotor gearbox, double gearing, double jKgMetersSquared
         super(gearbox, gearing, jKgMetersSquared, measurementStdDevs);
     }
 
+    public BetterDCMotorSim(DCMotor gearbox, double gearing, double jKgMetersSquared, PIDController pidController, SimpleMotorFeedforward feedforward) {
+        this(gearbox, gearing, jKgMetersSquared);
+        this.pidController = pidController;
+        this.feedforward = feedforward;
+    }
+
     @Override
     public void setInputVoltage(double volts) {
-        inputVoltage = volts;
-        setInput(volts);
+        inputVoltage = MathUtil.clamp(volts, -12, 12);
+        setInput(inputVoltage);
     }
 
     public double getInputVoltage() {
         return inputVoltage;
     }
+
+    public void setPosition(double setpoint) {
+        setPosition(setpoint, 0);
+    }
+
+    public void setPosition(double setpoint, double ffVolts) {
+        setInputVoltage(
+                pidController.calculate(getAngularPositionRotations(), setpoint)
+                + ffVolts
+        );
+    }
+
+    public void setVelocity(double setpoint) {
+        setInputVoltage(
+                feedforward.calculate(setpoint)
+                + pidController.calculate(getAngularVelocityRPS(), setpoint)
+        );
+    }
+
+    public double getAngularVelocityRPS() {
+        return Units.radiansToRotations(getAngularVelocityRadPerSec());
+    }
 }