Add swerve simulation to 2024-beta

src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java

@@ -89,6 +89,10 @@ public void setInverted(boolean inverted) {
         isInverted = inverted;
     }
 
+    public boolean getInverted() {
+        return isInverted;
+    }
+
     public REVLibError restoreFactoryDefaults() {
         return REVLibError.kOk;
     }

src/main/java/org/carlmontrobotics/lib199/sim/MockedCANCoder.java

@@ -34,7 +34,7 @@ public MockedCANCoder(CANcoder canCoder) {
     }
 
     public void update() {
-        sim.setRawPosition((int) (position.get() * kCANCoderCPR));
+        sim.setRawPosition(position.get() / kCANCoderCPR);
     }
 
     public void setGearing(double gearing) {

src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkEncoder.java

@@ -73,7 +73,7 @@ public void run() {
         double dCount = curCount - lastCount;
         lastTime = t;
         lastCount = curCount;
-        double newVelocity = velocityConversionFactor * ( dCount / dt ) / countsPerRevolution;
+        double newVelocity = velocityConversionFactor * ( dCount / dt ) / countsPerRevolution * 60;
         velocity = Double.isNaN(newVelocity) ? 0 : newVelocity;
     }
 

src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java

@@ -1,6 +1,9 @@
 package org.carlmontrobotics.lib199.swerve;
 
 
+import static edu.wpi.first.units.Units.Kilogram;
+import static edu.wpi.first.units.Units.Pounds;
+
 import java.util.function.Supplier;
 
 import org.mockito.internal.reporting.SmartPrinter;
@@ -20,6 +23,8 @@
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.units.Mass;
+import edu.wpi.first.units.Measure;
 import edu.wpi.first.util.sendable.Sendable;
 import edu.wpi.first.util.sendable.SendableBuilder;
 import edu.wpi.first.util.sendable.SendableRegistry;
@@ -449,4 +454,26 @@ public void initSendable(SendableBuilder builder) {
         builder.addDoubleProperty("Turn FF Output", () -> turnFFVolts, null);
         builder.addDoubleProperty("Turn Total Output", () -> turnVolts, null);
     }
+
+    /**
+     * Create and return a SwerveModuleSim that simulates the physics of this swerve module.
+     * 
+     * @param massOnWheel the mass on the wheel of this module (typically the mass of the robot divided by the number of modules)
+     * @param turnGearing the gearing reduction between the turn motor and the module
+     * @param turnMoiKgM2 the moment of inertia of the part of the module turned by the turn motor (in kg m^2)
+     * @return a SwerveModuleSim that simulates the physics of this swerve module.
+     */
+    public SwerveModuleSim createSim(Measure<Mass> massOnWheel, double turnGearing, double turnMoiKgM2) {
+        double driveMoiKgM2 =  massOnWheel.in(Kilogram) * Math.pow(config.wheelDiameterMeters/2, 2);
+        return new SwerveModuleSim(drive.getDeviceId(), config.driveGearing, drive.getInverted(), driveMoiKgM2, 
+            turn.getDeviceId(), turnEncoder.getDeviceID(), turnGearing, turn.getInverted(), turnMoiKgM2);
+    }
+
+    /**
+     * 
+     * @return a SwerveModuleSim that simulates this swerve module assuming it is one of 4 MK4i modules on our 114 lb 2024 robot.
+     */
+    public SwerveModuleSim createSim() {
+        return createSim(Pounds.of(114.0/4), 150.0/7, 0.0313);
+    }
 }

src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java

@@ -0,0 +1,71 @@
+package org.carlmontrobotics.lib199.swerve;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.units.Distance;
+import edu.wpi.first.units.Mass;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.Mult;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.simulation.DCMotorSim;
+import edu.wpi.first.wpilibj.simulation.SimDeviceSim;
+
+public class SwerveModuleSim {
+    private SimDeviceSim driveMotorSim, driveEncoderSim, turnMotorSim, turnEncoderSim;
+    private DCMotorSim drivePhysicsSim, turnPhysicsSim;
+    private double driveGearing, turnGearing;
+    private boolean driveInversion, turnInversion;
+    private Timer timer = new Timer();
+
+    /**
+     * Constructs a SwerveModuleSim that simulates the physics of a swerve module.
+     * 
+     * @param drivePortNum the port of the SparkMax drive motor
+     * @param driveGearing the gearing reduction between the drive motor and the wheel
+     * @param driveInversion whether the drive motor is inverted
+     * @param driveMoiKgM2 the effective moment of inertia around the wheel axle (typciall the mass of the robot divided the number of modules times the square of the wheel radius)
+     * @param turnMotorPortNum the port of the SparkMax turn motor
+     * @param turnEncoderPortNum the port of the CANCoder measuring the module's angle
+     * @param turnGearing the gearing reduction between the turn motor and the module
+     * @param turnInversion whether the turn motor is inverted
+     * @param turnMoiKgM2 the moment of inertia of the part of the module turned by the turn motor (in kg m^2)
+     */
+    public SwerveModuleSim(int drivePortNum, double driveGearing, boolean driveInversion, double driveMoiKgM2, 
+                            int turnMotorPortNum, int turnEncoderPortNum, double turnGearing, boolean turnInversion, double turnMoiKgM2) {
+        driveMotorSim = new SimDeviceSim("SparkMax", drivePortNum);
+        driveEncoderSim = new SimDeviceSim("RelativeEncoder", drivePortNum);
+        drivePhysicsSim = new DCMotorSim(DCMotor.getNEO(1), driveGearing, driveMoiKgM2);
+        this.driveGearing = driveGearing;
+        this.driveInversion = driveInversion;
+
+        turnMotorSim = new SimDeviceSim("SparkMax", turnMotorPortNum);
+        turnEncoderSim = new SimDeviceSim("CANCoder", turnEncoderPortNum);
+        turnPhysicsSim = new DCMotorSim(DCMotor.getNEO(1), turnGearing, turnMoiKgM2);
+        this.turnGearing = turnGearing;
+        this.turnInversion = turnInversion;
+    }
+
+    /**
+     * Steps the simulation forward by dtSecs seconds.
+     * @param dtSecs seconds to step the simulation forward.
+     */
+    public void update(double dtSecs) {
+        drivePhysicsSim.setInputVoltage(DriverStation.isEnabled() ? driveMotorSim.getDouble("Motor Output").get()*12.0 : 0.0);
+        drivePhysicsSim.update(dtSecs);
+        driveEncoderSim.getDouble("count").set((driveInversion ? -1.0: 1.0) * drivePhysicsSim.getAngularPositionRotations()*4096*driveGearing);
+
+        turnPhysicsSim.setInputVoltage(DriverStation.isEnabled() ? turnMotorSim.getDouble("Motor Output").get()*12.0 : 0.0);
+        turnPhysicsSim.update(dtSecs);
+        turnEncoderSim.getDouble("count").set(MathUtil.inputModulus((turnInversion ? -1.0: 1.0) * turnPhysicsSim.getAngularPositionRotations(), -0.5, 0.5)*4096);
+    }
+
+    /**
+     * Steps the simulation forward by the amount of time that has elapsed since this method was last called.
+     */
+    public void update() {
+        double dtSecs = timer.get();
+        timer.restart();
+        update(dtSecs);
+    }
+}