Merge pull request #94 from DeepBlueRobotics/fix-use-of-noncompliant-names-for-devices-and-data

Use spec compliant names for devices and data to fix issue #93.

Author: CoolSpy3

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

@@ -84,11 +84,24 @@ public void simulationPeriodic() {
         periodicSimulationMethods.forEach(RUN_RUNNABLE);
     }
 
-    public void asyncPeriodic() {
+    public synchronized void asyncPeriodic() {
         asyncPeriodicMethods.forEach(RUN_RUNNABLE);
         asyncPeriodicSimulationMethods.forEach(RUN_RUNNABLE);
     }
 
+    /**
+     * Unregisters all Runnables registered with registerAsyncPeriodic() and
+     * registerAsyncSimulationPeriodic(). Blocks until any currently registered
+     * Runnables have finished running. This is particularly useful for ensuring
+     * that Runnables registered in one test don't interfere with other tests.
+     */
+    public static void unregisterAllAsync() {
+        synchronized (INSTANCE) {
+            asyncPeriodicMethods.clear();
+            asyncPeriodicSimulationMethods.clear();
+        }
+    }
+
     private Lib199Subsystem() {}
 
 }

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

@@ -36,6 +36,7 @@ public class MockSparkBase extends MockedMotorBase {
     private SparkAbsoluteEncoder absoluteEncoder = null;
     private MockedEncoder alternateEncoder = null;
     private SparkAnalogSensor analogSensor = null;
+    private final String name;
 
     /**
      * Initializes a new {@link SimDevice} with the given parameters and creates the necessary sim values, and
@@ -44,14 +45,16 @@ public class MockSparkBase extends MockedMotorBase {
      * @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.
-     * @param name the name of the type of controller ("SparkMax" or "SparkFlex")
+     * @param name the name of the type of controller ("CANSparkMax" or "CANSparkFlex")
+     * @param countsPerRev the number of counts per revolution of this controller's built-in encoder.
      */
-    public MockSparkBase(int port, MotorType type, String name) {
+    public MockSparkBase(int port, MotorType type, String name, int countsPerRev) {
         super(name, port);
         this.type = type;
+        this.name = name;
 
         if(type == MotorType.kBrushless) {
-            encoder = new MockedEncoder(SimDevice.create(device.getName() + "_RelativeEncoder"), MockedEncoder.NEO_BUILTIN_ENCODER_CPR, false) {
+            encoder = new MockedEncoder(SimDevice.create("CANEncoder:" + name, port), countsPerRev, false, false) {
                 @Override
                 public REVLibError setInverted(boolean inverted) {
                     System.err.println(
@@ -60,7 +63,7 @@ public REVLibError setInverted(boolean inverted) {
                 }
             };
         } else {
-            encoder = new MockedEncoder(SimDevice.create(device.getName() + "_RelativeEncoder"), MockedEncoder.NEO_BUILTIN_ENCODER_CPR, false);
+            encoder = new MockedEncoder(SimDevice.create("CANEncoder:" + name, port), countsPerRev, false, false);
         }
 
         pidControllerImpl = new MockedSparkMaxPIDController(this);
@@ -192,9 +195,14 @@ public void close() {
      * @return the simulated encoder
      */
     public synchronized SparkAbsoluteEncoder getAbsoluteEncoder(SparkAbsoluteEncoder.Type encoderType) {
-        System.err.println("WARNING: An absolute encoder was created for a simulated Spark Max. Currently, the only way to specify the CPR is to use the REVHardwareClient. A CPR of " + MockedEncoder.NEO_BUILTIN_ENCODER_CPR + " will be assumed.");
         if(absoluteEncoder == null) {
-            MockedEncoder absoluteEncoderImpl = new MockedEncoder(SimDevice.create(device.getName() + "_AbsoluteEncoder"), MockedEncoder.NEO_BUILTIN_ENCODER_CPR, true);
+            MockedEncoder absoluteEncoderImpl = new MockedEncoder(SimDevice.create("CANDutyCycle:" + name, port), 0, false, true) {
+                @Override
+                public double getVelocity() {
+                    // A SparkAbsoluteEncoder returns a velocity in rps, not rpm.
+                    return super.getVelocity() / 60.0;
+                }            
+            };
             absoluteEncoder = Mocks.createMock(SparkAbsoluteEncoder.class, absoluteEncoderImpl, new REVLibErrorAnswer());
         }
         return absoluteEncoder;
@@ -223,7 +231,7 @@ public RelativeEncoder getAlternateEncoder(int countsPerRev) {
      */
     public synchronized RelativeEncoder getAlternateEncoder(SparkMaxAlternateEncoder.Type encoderType, int countsPerRev) {
         if(alternateEncoder == null) {
-            alternateEncoder = new MockedEncoder(SimDevice.create(device.getName() + "_AlternateEncoder"), countsPerRev, false);
+            alternateEncoder = new MockedEncoder(SimDevice.create("CANEncoder:%s[%d]-alternate".formatted(name, port)), 0, false, false);
         }
         return alternateEncoder;
     }
@@ -239,7 +247,7 @@ public synchronized RelativeEncoder getAlternateEncoder(SparkMaxAlternateEncoder
      */
     public synchronized SparkAnalogSensor getAnalog(SparkAnalogSensor.Mode mode) {
         if(analogSensor == null) {
-            MockedEncoder analogSensorImpl = new MockedEncoder(SimDevice.create(device.getName() + "_AnalogSensor"), MockedEncoder.ANALOG_SENSOR_MAX_VOLTAGE, true);
+            MockedEncoder analogSensorImpl = new MockedEncoder(SimDevice.create("CANAIn:" + name, port), 0, true, true);
             analogSensor = Mocks.createMock(SparkAnalogSensor.class, analogSensorImpl, new REVLibErrorAnswer());
         }
         return analogSensor;
@@ -268,6 +276,10 @@ public REVLibError setIdleMode(IdleMode mode) {
         return REVLibError.kOk;
     }
 
+    public double getOutputCurrent() {
+        return getCurrentDraw();
+    }
+
     @Override
     public void disable() {
         // CANSparkBase sets the motor speed to zero rather than actually disabling the motor

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

@@ -9,7 +9,7 @@
 public class MockSparkFlex extends MockSparkBase {
 
     public MockSparkFlex(int port, MotorType type) {
-        super(port, type, "SparkFlex");
+        super(port, type, "CANSparkFlex", 7168);
     }
 
     public static CANSparkFlex createMockSparkFlex(int portPWM, MotorType type) {

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

@@ -9,7 +9,7 @@
 public class MockSparkMax extends MockSparkBase {
 
     public MockSparkMax(int port, MotorType type) {
-        super(port, type, "SparkMax");
+        super(port, type, "CANSparkMax", 42);
     }
 
     public static CANSparkMax createMockSparkMax(int portPWM, MotorType type) {

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

@@ -5,9 +5,8 @@
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.ctre.phoenix6.sim.CANcoderSimState;
 
-import org.carlmontrobotics.lib199.Lib199Subsystem;
-
 import edu.wpi.first.hal.HALValue;
+import edu.wpi.first.hal.SimBoolean;
 import edu.wpi.first.hal.SimDevice;
 import edu.wpi.first.hal.SimDevice.Direction;
 import edu.wpi.first.hal.simulation.SimValueCallback;
@@ -16,39 +15,27 @@
 
 public class MockedCANCoder {
 
-    public static final double kCANCoderCPR = 4096;
-
     private static final HashMap<Integer, MockedCANCoder> sims = new HashMap<>();
 
     private int port;
     private SimDevice device;
     private SimDeviceSim deviceSim;
     private SimDouble position; // Rotations - Continuous
-    private SimDouble gearing;
     private CANcoderSimState sim;
 
     public MockedCANCoder(CANcoder canCoder) {
         port = canCoder.getDeviceID();
-        device = SimDevice.create("CANCoder", port);
-        position = device.createDouble("count", Direction.kInput, 0);
-        gearing = device.createDouble("gearing", Direction.kOutput, 1);
+        device = SimDevice.create("CANDutyCycle:CANCoder", port);
+        position = device.createDouble("position", Direction.kInput, 0);
         sim = canCoder.getSimState();
-        deviceSim = new SimDeviceSim("CANCoder", port);
+        deviceSim = new SimDeviceSim("CANDutyCycle:CANCoder", port);
         deviceSim.registerValueChangedCallback(position, new SimValueCallback() {
             @Override
             public void callback(String name, int handle, int direction, HALValue value) {
-                sim.setRawPosition(value.getDouble() / kCANCoderCPR);
+                sim.setRawPosition(value.getDouble());
             }
         }, true);
         sims.put(port, this);
     }
 
-    public void setGearing(double gearing) {
-        this.gearing.set(gearing);
-    }
-
-    public static void setGearing(int port, double gearing) {
-        if(sims.containsKey(port)) sims.get(port).setGearing(gearing);
-    }
-
 }

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

@@ -5,6 +5,7 @@
 import com.revrobotics.REVLibError;
 import com.revrobotics.RelativeEncoder;
 
+import edu.wpi.first.hal.SimBoolean;
 import edu.wpi.first.hal.SimDevice;
 import edu.wpi.first.hal.SimDevice.Direction;
 import edu.wpi.first.math.MathUtil;
@@ -22,13 +23,14 @@
  */
 public class MockedEncoder implements AbsoluteEncoder, AnalogInput, AutoCloseable, RelativeEncoder {
 
-    public static final int NEO_BUILTIN_ENCODER_CPR = 42;
     public static final double ANALOG_SENSOR_MAX_VOLTAGE = 3.3;
 
     public final SimDevice device;
     protected final SimDouble position;
     protected final SimDouble velocity;
-    protected final double countsOrVoltsPerRev;
+    protected final SimDouble voltage;
+    protected final SimBoolean init;
+    protected final int countsPerRev;
     protected final boolean absolute;
     protected double positionConversionFactor = 1.0;
     protected double velocityConversionFactor = 1.0;
@@ -37,17 +39,24 @@ public class MockedEncoder implements AbsoluteEncoder, AnalogInput, AutoCloseabl
 
     /**
      * @param device The device to retrieve position and velocity data from
-     * @param countsOrVoltsPerRev The cpr of the simulated encoder
+     * @param countsPerRev The value that this.getCountsPerRevolution() should return
+     * @param analog Whether the encoder is an analog sensor
      * @param absolute Whether the encoder is an absolute encoder.
      * This flag caps the position to one rotation via. {@link MathUtil#inputModulus(double, double, double)},
      * disables {@link #setPosition(double)}, and enables {@link #setZeroOffset(double)}.
      */
-    public MockedEncoder(SimDevice device, double countsOrVoltsPerRev, boolean absolute) {
+    public MockedEncoder(SimDevice device, int countsPerRev, boolean analog, boolean absolute) {
         this.device = device;
-        position = device.createDouble("Position", Direction.kInput, 0);
-        velocity = device.createDouble("Velocity", Direction.kInput, 0);
-        this.countsOrVoltsPerRev = countsOrVoltsPerRev;
+        position = device.createDouble("position", Direction.kInput, 0); // Rotations
+        velocity = device.createDouble("velocity", Direction.kInput, 0); // Rotations per *second*
+        if (analog) {
+            voltage = device.createDouble("voltage", Direction.kInput, 0);
+        } else {
+            voltage = null;
+        }
+        this.countsPerRev = countsPerRev;
         this.absolute = absolute;
+        init = device.createBoolean("init", Direction.kOutput, true);
     }
 
     @Override
@@ -74,14 +83,15 @@ public int getMeasurementPeriod() {
 
     @Override
     public int getCountsPerRevolution() {
-        return (int)countsOrVoltsPerRev;
+        return countsPerRev;
     }
 
     /**
      * @return The current position of the encoder, not accounting for the position offset ({@link #setPosition(double)} and {@link #setZeroOffset(double)})
      */
     public double getRawPosition() {
-        return position.get() * (inverted ? -1 : 1) * positionConversionFactor / countsOrVoltsPerRev;
+        double rotationsOrVolts = voltage != null ? voltage.get() : position.get();
+        return rotationsOrVolts * (inverted ? -1 : 1) * positionConversionFactor;
     }
 
     @Override
@@ -95,7 +105,7 @@ public double getPosition() {
 
     @Override
     public double getVelocity() {
-        return velocity.get() * (inverted ? -1 : 1) * velocityConversionFactor / countsOrVoltsPerRev;
+        return velocity.get() * 60 * (inverted ? -1 : 1) * velocityConversionFactor;
     }
 
     @Override
@@ -160,14 +170,13 @@ public double getZeroOffset() {
 
     @Override
     public void close() {
+        init.set(false);
         device.close();
     }
 
     @Override
     public double getVoltage() {
-        // This method only makes sense for an analog sensor and for an analog sensor, 
-        // position.get() is supposed to return volts.
-        return position.get();
+        return voltage.get();
     }
 
 }

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

@@ -26,6 +26,7 @@ public abstract class MockedMotorBase implements AutoCloseable, MotorController,
     public final SimDouble neutralDeadband;
     public final SimBoolean brakeModeEnabled;
     public final SimDouble currentDraw;
+    public final SimDouble busVoltage;
     protected SlewRateLimiter rampRateLimiter = null;
     protected boolean isInverted = false;
     protected boolean disabled = false;
@@ -42,12 +43,13 @@ public abstract class MockedMotorBase implements AutoCloseable, MotorController,
      * @param port the device port to pass to {@link SimDevice#create}
      */
     public MockedMotorBase(String type, int port) {
-        device = SimDevice.create(type, port);
+        device = SimDevice.create("CANMotor:" + type, port);
         this.port = port;
-        speed = device.createDouble("Speed", Direction.kOutput, 0.0);
-        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);
+        speed = device.createDouble("percentOutput", Direction.kOutput, 0.0);
+        neutralDeadband = device.createDouble("neutralDeadband", Direction.kOutput, 0.04);
+        brakeModeEnabled = device.createBoolean("brakeMode", Direction.kOutput, true);
+        currentDraw = device.createDouble("motorCurrent", Direction.kInput, 0.0);
+        busVoltage = device.createDouble("busVoltage", Direction.kInput, defaultNominalVoltage);
     }
 
     /**
@@ -137,7 +139,7 @@ public void doEnableVoltageCompensation(double nominalVoltage) {
 	}
 
 	public void doDisableVoltageCompensation() {
-        voltageCompensationNominalVoltage = defaultNominalVoltage;
+        voltageCompensationNominalVoltage = busVoltage.get();
         setRampRate(getRampRate()); // Update the ramp rate to account for the new nominal voltage
     }
 
@@ -151,7 +153,7 @@ public double getCurrentDraw() {
 
     public void updateRequestedSpeed() {
         double percent = getRequestedSpeed();
-        percent *= voltageCompensationNominalVoltage / defaultNominalVoltage;
+        percent *= voltageCompensationNominalVoltage / busVoltage.get();
         percent *= isInverted ? -1.0 : 1.0;
         requestedSpeedPercent = percent;
     }

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

@@ -8,6 +8,7 @@
 import com.playingwithfusion.TimeOfFlight;
 import com.playingwithfusion.TimeOfFlight.RangingMode;
 
+import edu.wpi.first.hal.SimBoolean;
 import edu.wpi.first.hal.SimDevice;
 import edu.wpi.first.hal.SimDevice.Direction;
 import edu.wpi.first.hal.SimDouble;
@@ -17,33 +18,39 @@
 public class MockedPlayingWithFusionTimeOfFlight implements AutoCloseable {
 
     private int port;
-    private SimDevice device;
+    private SimDevice rangeDevice;
+    private SimDevice ambientLightLevelDevice;
     private SimDouble range, rangeSigma, sampleTime, ambientLightLevel;
+    private SimBoolean rangeDeviceInit, ambientLightLevelDeviceInit;
     private SimInt roiLeft, roiTop, roiRight, roiBottom;
     private SimEnum status;
     private SimEnum rangingMode;
 
     public MockedPlayingWithFusionTimeOfFlight(int portNumber) {
         port = portNumber;
-        device = SimDevice.create("PlayingWithFusionTimeOfFlight", port);
-        range = device.createDouble("range", Direction.kInput, 0);
-        rangeSigma = device.createDouble("rangeSigma", Direction.kInput, 1);
-        sampleTime = device.createDouble("sampleTime", Direction.kBidir, 24);
+        rangeDevice = SimDevice.create("CANAIn:PlayingWithFusionTimeOfFlight[%d]-rangeVoltsIsMM".formatted(port));
+        range = rangeDevice.createDouble("voltage", Direction.kInput, 0); // Millimeters
+        rangeSigma = rangeDevice.createDouble("rangeSigma", Direction.kInput, 1); // Millimeters
+        sampleTime = rangeDevice.createDouble("sampleTime", Direction.kBidir, 24); // Milliseconds
 
         // Note: default ambientLightLevel of 0.005*16*16 Mcps is typical for office lighting per the vl5311x datasheet:
         // https://www.playingwithfusion.com/include/getfile.php?fileid=7073
-        ambientLightLevel = device.createDouble("ambientLightLevel", Direction.kInput, 0.005*16*16);
+        ambientLightLevelDevice = SimDevice.create("CANAIn:PlayingWithFusionTimeOfFlight[%d]-ambientLightLevelVoltsIsMcps".formatted(port));
+        ambientLightLevel = ambientLightLevelDevice.createDouble("voltage", Direction.kInput, 0.005*16*16);
 
         String[] statusNames = Arrays.stream(Status.values()).map(Status::name).toArray(String[]::new);
-        status = device.createEnum("status", Direction.kInput, statusNames, Status.Invalid.ordinal());
+        status = rangeDevice.createEnum("status", Direction.kInput, statusNames, Status.Invalid.ordinal());
 
         String[] rangingModeNames = Arrays.stream(RangingMode.values()).map(RangingMode::name).toArray(String[]::new);
-        rangingMode = device.createEnum("rangingMode", Direction.kInput, rangingModeNames, RangingMode.Short.ordinal());
+        rangingMode = rangeDevice.createEnum("rangingMode", Direction.kInput, rangingModeNames, RangingMode.Short.ordinal());
 
-        roiLeft = device.createInt("roiLeft", Direction.kOutput, 0);
-        roiTop = device.createInt("roiTop", Direction.kOutput, 0);
-        roiRight = device.createInt("roiRight", Direction.kOutput, 15);
-        roiBottom = device.createInt("roiBottom", Direction.kOutput, 15);
+        roiLeft = rangeDevice.createInt("roiLeft", Direction.kOutput, 0);
+        roiTop = rangeDevice.createInt("roiTop", Direction.kOutput, 0);
+        roiRight = rangeDevice.createInt("roiRight", Direction.kOutput, 15);
+        roiBottom = rangeDevice.createInt("roiBottom", Direction.kOutput, 15);
+
+        rangeDeviceInit = rangeDevice.createBoolean("init", Direction.kOutput, true);
+        ambientLightLevelDeviceInit = ambientLightLevelDevice.createBoolean("init", Direction.kOutput, true);
     }
 
     public static TimeOfFlight createMock(int portNumber) {
@@ -96,6 +103,9 @@ public void setRangingMode(RangingMode newMode, double newSampleTime) {
 
     @Override
     public void close() {
-        device.close();
+        rangeDeviceInit.set(false);
+        rangeDevice.close();
+        ambientLightLevelDeviceInit.set(false);
+        ambientLightLevelDevice.close();
     }
 }