Fix swerve module sim

src/main/java/org/carlmontrobotics/lib199/Lib199Subsystem.java

@@ -24,10 +24,15 @@ public class Lib199Subsystem implements Subsystem {
 
         asyncPeriodicThread = new Thread(() -> {
             while(true) {
-                INSTANCE.asyncPeriodic();
                 try {
-                    Thread.sleep(asyncSleepTime);
-                } catch(InterruptedException e) {}
+                    INSTANCE.asyncPeriodic();
+                    try {
+                        Thread.sleep(asyncSleepTime);
+                    } catch(InterruptedException e) {}
+                } catch (Exception ex) {
+                    System.err.println("Lib199 error running ayncPeriodic() methods: " + ex);
+                    ex.printStackTrace(System.err);
+                }
             }
         });
         asyncPeriodicThread.setDaemon(true);

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

@@ -3,6 +3,7 @@
 import java.util.concurrent.ConcurrentHashMap;
 
 import org.carlmontrobotics.lib199.DummySparkMaxAnswer;
+import org.carlmontrobotics.lib199.Lib199Subsystem;
 import org.carlmontrobotics.lib199.Mocks;
 import org.carlmontrobotics.lib199.REVLibErrorAnswer;
 
@@ -38,6 +39,9 @@ public class MockSparkMax extends MockedMotorBase {
     private SparkAnalogSensor analogSensor = null;
 
     /**
+     * Initializes a new {@link SimDevice} with the given parameters and creates the necessary sim values, and
+     * registers this class's {@link #run()} method to be called asynchronously via {@link Lib199Subsystem#registerAsyncSimulationPeriodic(Runnable)}.
+     * 
      * @param port the port to associate this {@code MockSparkMax} with. Will be used to create the {@link SimDevice} and facilitate motor following.
      * @param type the type of the simulated motor. If this is set to {@link MotorType#kBrushless}, the builtin encoder simulation will be configured
      * to follow the inversion state of the motor and its {@code setInverted} method will be disabled.
@@ -64,6 +68,8 @@ public REVLibError setInverted(boolean inverted) {
         pidController.setFeedbackDevice(encoder);
 
         controllers.put(port, this);
+
+        Lib199Subsystem.registerAsyncSimulationPeriodic(this);
     }
 
     @Override
@@ -93,7 +99,6 @@ public static CANSparkMax createMockSparkMax(int port, MotorType type) {
     @Override
     public void set(double speed) {
         speed *= voltageCompensationNominalVoltage / defaultNominalVoltage;
-        speed = (isInverted ? -1.0 : 1.0) * speed;
         pidControllerImpl.setDutyCycle(speed);
     }
 

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

@@ -6,7 +6,6 @@
 import edu.wpi.first.hal.SimDouble;
 import edu.wpi.first.math.filter.SlewRateLimiter;
 import edu.wpi.first.wpilibj.motorcontrol.MotorController;
-import org.carlmontrobotics.lib199.Lib199Subsystem;
 
 /**
  * Represents a base encoder class which can connect to a DeepBlueSim SimDeviceMotorMediator.
@@ -37,8 +36,7 @@ public abstract class MockedMotorBase implements AutoCloseable, MotorController,
     private double requestedSpeedPercent = 0.0;
 
     /**
-     * Initializes a new {@link SimDevice} with the given parameters, creates the necessary sim values, and
-     * registers this class's {@link #run()} method to be called asynchronously via {@link Lib199Subsystem#registerAsyncSimulationPeriodic(Runnable)}.
+     * Initializes a new {@link SimDevice} with the given parameters and creates the necessary sim values.
      *
      * @param type the device type name to pass to {@link SimDevice#create}
      * @param port the device port to pass to {@link SimDevice#create}
@@ -50,8 +48,6 @@ public MockedMotorBase(String type, int port) {
         neutralDeadband = device.createDouble("Neutral Deadband", Direction.kOutput, 0.04);
         brakeModeEnabled = device.createBoolean("Brake Mode", Direction.kOutput, true);
         currentDraw = device.createDouble("Current Draw", Direction.kInput, 0.0);
-
-        Lib199Subsystem.registerAsyncSimulationPeriodic(this);
     }
 
     /**

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

@@ -1,5 +1,8 @@
 package org.carlmontrobotics.lib199.swerve;
 
+import org.carlmontrobotics.lib199.sim.MockedCANCoder;
+import org.carlmontrobotics.lib199.sim.MockedEncoder;
+
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.units.Distance;
@@ -34,7 +37,7 @@ public class SwerveModuleSim {
     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);
+        driveEncoderSim = new SimDeviceSim(driveMotorSim.getName() + "_RelativeEncoder");
         drivePhysicsSim = new DCMotorSim(DCMotor.getNEO(1), driveGearing, driveMoiKgM2);
         this.driveGearing = driveGearing;
         this.driveInversion = driveInversion;
@@ -51,13 +54,18 @@ public SwerveModuleSim(int drivePortNum, double driveGearing, boolean driveInver
      * @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.setInputVoltage(DriverStation.isEnabled() ? driveMotorSim.getDouble("Speed").get()*12.0 : 0.0);
         drivePhysicsSim.update(dtSecs);
-        driveEncoderSim.getDouble("count").set((driveInversion ? -1.0: 1.0) * drivePhysicsSim.getAngularPositionRotations()*4096*driveGearing);
+        driveEncoderSim.getDouble("Position").set(drivePhysicsSim.getAngularPositionRotations()*MockedEncoder.NEO_BUILTIN_ENCODER_CPR*driveGearing);
+        driveEncoderSim.getDouble("Velocity").set(drivePhysicsSim.getAngularVelocityRPM()*MockedEncoder.NEO_BUILTIN_ENCODER_CPR*driveGearing);
 
-        turnPhysicsSim.setInputVoltage(DriverStation.isEnabled() ? turnMotorSim.getDouble("Motor Output").get()*12.0 : 0.0);
+        turnPhysicsSim.setInputVoltage(DriverStation.isEnabled() ? turnMotorSim.getDouble("Speed").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);
+        // The -1.0 below is to account for the fact that the CANCoder is mounted such that turning the wheel CCW (as viewed from above) causes
+        // the encoder value to decrease. However the turnPhysicsSim's angular position will *increase* under those circumstances.
+        // Note that this is independent of turnInversion. turnInversion controls which direction a positive voltage will cause the turn motor
+        // to spin. turnInversion should be used to set the motor's inversion so that a positive voltage will spin the wheel CCW (as viewed from above).
+        turnEncoderSim.getDouble("count").set(MathUtil.inputModulus(-1.0 * turnPhysicsSim.getAngularPositionRotations(), -0.5, 0.5)*MockedCANCoder.kCANCoderCPR);
     }
 
     /**

src/test/java/org/carlmontrobotics/lib199/sim/MockSparkMaxTest.java

@@ -0,0 +1,42 @@
+package org.carlmontrobotics.lib199.sim;
+
+import static org.junit.Assert.assertEquals;
+import static org.junit.Assert.assertFalse;
+import static org.junit.Assert.assertNotNull;
+import static org.junit.Assert.assertTrue;
+
+import java.util.stream.Stream;
+
+import com.revrobotics.REVLibError;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+
+import org.carlmontrobotics.lib199.Mocks;
+import org.carlmontrobotics.lib199.REVLibErrorAnswer;
+import org.carlmontrobotics.lib199.testUtils.SafelyClosable;
+import org.carlmontrobotics.lib199.testUtils.TestRules;
+import org.junit.ClassRule;
+import org.junit.Rule;
+import org.junit.Test;
+
+import edu.wpi.first.hal.SimDevice;
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.wpilibj.simulation.SimDeviceSim;
+
+public class MockSparkMaxTest {
+    @ClassRule
+    public static TestRules.InitializeHAL simClassRule = new TestRules.InitializeHAL(); 
+    @Rule
+    public TestRules.ResetSimDeviceSimData simTestRule = new TestRules.ResetSimDeviceSimData(); 
+
+    @Test
+    public void testHasEncoder() {
+        var mockSpark = new MockSparkMax(0, MotorType.kBrushless);
+        SimDeviceSim simSpark = new SimDeviceSim("SparkMax", 0);
+        assertNotNull(simSpark);
+        SimDeviceSim simEncoder = new SimDeviceSim(simSpark.getName() + "_RelativeEncoder");
+        assertNotNull(simEncoder);
+        SimDouble simPosition = simEncoder.getDouble("Position");
+        assertNotNull(simPosition);
+    }
+}