diff --git a/src/main/java/org/carlmontrobotics/lib199/Lib199Subsystem.java b/src/main/java/org/carlmontrobotics/lib199/Lib199Subsystem.java index efc64d49..c8e242ed 100644 --- a/src/main/java/org/carlmontrobotics/lib199/Lib199Subsystem.java +++ b/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() {} } diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkBase.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkBase.java index cc63577b..36ef4966 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkBase.java +++ b/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 diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkFlex.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkFlex.java index 0a7991a3..47fb3a11 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkFlex.java +++ b/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) { diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java index 55805d61..653862e9 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java +++ b/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) { diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockedCANCoder.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockedCANCoder.java index ee39a9c5..02dfd603 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockedCANCoder.java +++ b/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 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); - } - } \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockedEncoder.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockedEncoder.java index 23deaf32..7bcff878 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockedEncoder.java +++ b/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(); } } diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockedMotorBase.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockedMotorBase.java index 40b9e490..dcba5a7d 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockedMotorBase.java +++ b/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; } diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockedPlayingWithFusionTimeOfFlight.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockedPlayingWithFusionTimeOfFlight.java index 58e9597d..6d5805dc 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockedPlayingWithFusionTimeOfFlight.java +++ b/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(); } } diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java index 26485eaa..d39dc473 100644 --- a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java @@ -465,8 +465,8 @@ public void initSendable(SendableBuilder builder) { */ public SwerveModuleSim createSim(Measure 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 new SwerveModuleSim(drive.getDeviceId(), config.driveGearing, driveMoiKgM2, + turn.getDeviceId(), turnEncoder.getDeviceID(), turnGearing, turnMoiKgM2); } /** diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java index 8ddc1167..14c52890 100644 --- a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java @@ -17,8 +17,7 @@ public class SwerveModuleSim { private SimDeviceSim driveMotorSim, driveEncoderSim, turnMotorSim, turnEncoderSim; private DCMotorSim drivePhysicsSim, turnPhysicsSim; - private double driveGearing, turnGearing; - private boolean driveInversion, turnInversion; + private double driveGearing; private Timer timer = new Timer(); /** @@ -26,27 +25,22 @@ public class SwerveModuleSim { * * @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(driveMotorSim.getName() + "_RelativeEncoder"); + public SwerveModuleSim(int drivePortNum, double driveGearing, double driveMoiKgM2, + int turnMotorPortNum, int turnEncoderPortNum, double turnGearing, double turnMoiKgM2) { + driveMotorSim = new SimDeviceSim("CANMotor:CANSparkMax", drivePortNum); + driveEncoderSim = new SimDeviceSim("CANEncoder:CANSparkMax", 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); + turnMotorSim = new SimDeviceSim("CANMotor:CANSparkMax", turnMotorPortNum); + turnEncoderSim = new SimDeviceSim("CANDutyCycle:CANCoder", turnEncoderPortNum); turnPhysicsSim = new DCMotorSim(DCMotor.getNEO(1), turnGearing, turnMoiKgM2); - this.turnGearing = turnGearing; - this.turnInversion = turnInversion; } /** @@ -54,18 +48,16 @@ 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("Speed").get()*12.0 : 0.0); + drivePhysicsSim.setInputVoltage(DriverStation.isEnabled() ? driveMotorSim.getDouble("percentOutput").get()*12.0 : 0.0); drivePhysicsSim.update(dtSecs); - driveEncoderSim.getDouble("Position").set(drivePhysicsSim.getAngularPositionRotations()*MockedEncoder.NEO_BUILTIN_ENCODER_CPR*driveGearing); - driveEncoderSim.getDouble("Velocity").set(drivePhysicsSim.getAngularVelocityRPM()*MockedEncoder.NEO_BUILTIN_ENCODER_CPR*driveGearing); + driveEncoderSim.getDouble("position").set(drivePhysicsSim.getAngularPositionRotations()*driveGearing); + driveEncoderSim.getDouble("velocity").set(drivePhysicsSim.getAngularVelocityRPM()/60.0*driveGearing); - turnPhysicsSim.setInputVoltage(DriverStation.isEnabled() ? turnMotorSim.getDouble("Speed").get()*12.0 : 0.0); + turnPhysicsSim.setInputVoltage(DriverStation.isEnabled() ? turnMotorSim.getDouble("percentOutput").get()*12.0 : 0.0); turnPhysicsSim.update(dtSecs); // 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); + turnEncoderSim.getDouble("position").set(MathUtil.inputModulus(-1.0 * turnPhysicsSim.getAngularPositionRotations(), -0.5, 0.5)); } /** diff --git a/src/test/java/org/carlmontrobotics/lib199/sim/MockSparkMaxTest.java b/src/test/java/org/carlmontrobotics/lib199/sim/MockSparkMaxTest.java index e4e0d07d..d8837fcb 100644 --- a/src/test/java/org/carlmontrobotics/lib199/sim/MockSparkMaxTest.java +++ b/src/test/java/org/carlmontrobotics/lib199/sim/MockSparkMaxTest.java @@ -1,5 +1,6 @@ package org.carlmontrobotics.lib199.sim; +import static org.junit.Assert.assertEquals; import static org.junit.Assert.assertNotNull; import com.revrobotics.CANSparkLowLevel.MotorType; @@ -21,11 +22,22 @@ public class MockSparkMaxTest { @Test public void testHasEncoder() { var mockSpark = new MockSparkMax(0, MotorType.kBrushless); - SimDeviceSim simSpark = new SimDeviceSim("SparkMax", 0); + SimDeviceSim simSpark = new SimDeviceSim("CANMotor:CANSparkMax", 0); assertNotNull(simSpark); - SimDeviceSim simEncoder = new SimDeviceSim(simSpark.getName() + "_RelativeEncoder"); + SimDeviceSim simEncoder = new SimDeviceSim("CANEncoder:CANSparkMax", 0); assertNotNull(simEncoder); - SimDouble simPosition = simEncoder.getDouble("Position"); + SimDouble simPosition = simEncoder.getDouble("position"); assertNotNull(simPosition); } + + @Test + public void testGetOutputCurrent() { + var mockSpark = new MockSparkMax(0, MotorType.kBrushless); + SimDeviceSim simSpark = new SimDeviceSim("CANMotor:CANSparkMax", 0); + assertNotNull(simSpark); + SimDouble simCurrent = simSpark.getDouble("motorCurrent"); + assertNotNull(simCurrent); + simCurrent.set(42.0); + assertEquals(42, mockSpark.getOutputCurrent(), 1e-6); + } } diff --git a/src/test/java/org/carlmontrobotics/lib199/sim/MockedCANCoderTest.java b/src/test/java/org/carlmontrobotics/lib199/sim/MockedCANCoderTest.java index 4790379d..37e13a4e 100644 --- a/src/test/java/org/carlmontrobotics/lib199/sim/MockedCANCoderTest.java +++ b/src/test/java/org/carlmontrobotics/lib199/sim/MockedCANCoderTest.java @@ -45,8 +45,8 @@ public void testCountUpdatesPosition() { assertPositionEqualsWithinTime(canCoder, 0.0, timeoutSec, delta); // Set the position to 0.42 rotations via the SimDevice interface - SimDeviceSim canCoderSim = new SimDeviceSim("CANCoder", 0); - canCoderSim.getDouble("count").set(0.42 * MockedCANCoder.kCANCoderCPR); + SimDeviceSim canCoderSim = new SimDeviceSim("CANDutyCycle:CANCoder", 0); + canCoderSim.getDouble("position").set(0.42); assertPositionEqualsWithinTime(canCoder, 0.42, timeoutSec, delta); } diff --git a/src/test/java/org/carlmontrobotics/lib199/sim/MockedEncoderTest.java b/src/test/java/org/carlmontrobotics/lib199/sim/MockedEncoderTest.java index 99cb1fd1..1f842f56 100644 --- a/src/test/java/org/carlmontrobotics/lib199/sim/MockedEncoderTest.java +++ b/src/test/java/org/carlmontrobotics/lib199/sim/MockedEncoderTest.java @@ -45,26 +45,26 @@ private void assertTestDeviceCreation(int id) { assertEquals(1, Stream.of(sim.enumerateValues()) .map(info -> info.name) .distinct() - .filter(name -> name.equals("Position")).count()); + .filter(name -> name.equals("position")).count()); } assertFalse(simDeviceExists(deviceName)); } @Test public void testFunctionality() { - withEncoders((enc, sim, count) -> { - testFunctionalityWithPositionConversionFactor(1, enc, count); - testFunctionalityWithPositionConversionFactor(10, enc, count); - testFunctionalityWithPositionConversionFactor(100, enc, count); + withEncoders((enc, sim, positionSim) -> { + testFunctionalityWithPositionConversionFactor(1, enc, positionSim); + testFunctionalityWithPositionConversionFactor(10, enc, positionSim); + testFunctionalityWithPositionConversionFactor(100, enc, positionSim); }); } - private void testFunctionalityWithPositionConversionFactor(double factor, RelativeEncoder enc, SimDouble count) { + private void testFunctionalityWithPositionConversionFactor(double factor, RelativeEncoder enc, SimDouble positionSim) { assertEquals(REVLibError.kOk, enc.setPositionConversionFactor(factor)); assertEquals(factor, enc.getPositionConversionFactor(), 0.01); - testPosition(10, enc, factor, count); - testPosition(0, enc, factor, count); - testPosition(-10, enc, factor, count); + testPosition(10, enc, factor, positionSim); + testPosition(0, enc, factor, positionSim); + testPosition(-10, enc, factor, positionSim); } private void testPosition(double position, RelativeEncoder enc, double conversionFactor, SimDouble positionSim) { @@ -73,7 +73,7 @@ private void testPosition(double position, RelativeEncoder enc, double conversio assertEquals(position, enc.getPosition(), 0.02); assertEquals(REVLibError.kOk, enc.setPosition(0)); assertEquals(0, enc.getPosition(), 0.02); - positionSim.set(position / enc.getPositionConversionFactor() * enc.getCountsPerRevolution() + positionSim.get()); + positionSim.set(position / enc.getPositionConversionFactor() + positionSim.get()); assertEquals(position, enc.getPosition(), 0.02); } @@ -89,7 +89,7 @@ private SafelyClosable createEncoder(int deviceId) { SimDevice device = SimDevice.create("testDevice", deviceId); return (SafelyClosable)Mocks.createMock( RelativeEncoder.class, - new MockedEncoder(device, 4096, false), + new MockedEncoder(device, 4096, false, false), new REVLibErrorAnswer(), SafelyClosable.class); } @@ -103,14 +103,14 @@ private void withEncoders(EncoderTest func) { private void withEncoder(int id, EncoderTest func) { try(SafelyClosable encoder = createEncoder(id)) { SimDeviceSim sim = new SimDeviceSim("testDevice", id); - SimDouble count = sim.getDouble("Position"); - assertNotNull(count); - func.test((RelativeEncoder)encoder, sim, count); + SimDouble posSim = sim.getDouble("position"); + assertNotNull(posSim); + func.test((RelativeEncoder)encoder, sim, posSim); } } private interface EncoderTest { - public void test(RelativeEncoder encoder, SimDeviceSim sim, SimDouble count); + public void test(RelativeEncoder encoder, SimDeviceSim sim, SimDouble posSim); } } diff --git a/src/test/java/org/carlmontrobotics/lib199/sim/MockedPlayingWithFusionTimeOfFlightTest.java b/src/test/java/org/carlmontrobotics/lib199/sim/MockedPlayingWithFusionTimeOfFlightTest.java index 82e84275..a0b3d806 100644 --- a/src/test/java/org/carlmontrobotics/lib199/sim/MockedPlayingWithFusionTimeOfFlightTest.java +++ b/src/test/java/org/carlmontrobotics/lib199/sim/MockedPlayingWithFusionTimeOfFlightTest.java @@ -40,21 +40,21 @@ public void testDeviceCreation() { } private void assertTestDeviceCreation(int id) { - String deviceName = String.format("PlayingWithFusionTimeOfFlight[%d]", id); + String deviceName = String.format("CANAIn:PlayingWithFusionTimeOfFlight[%d]-rangeVoltsIsMM", id); assertFalse(simDeviceExists(deviceName)); try(TimeOfFlight dev = createDevice(id)) { assertTrue(simDeviceExists(deviceName)); - SimDeviceSim sim = new SimDeviceSim("PlayingWithFusionTimeOfFlight", id); + SimDeviceSim sim = new SimDeviceSim(deviceName); var valueNames = Stream.of(sim.enumerateValues()).map(info -> info.name).toList(); - assertThat(valueNames, hasItems("range", "rangeSigma", "sampleTime", "ambientLightLevel", "status", "rangingMode", "roiLeft", "roiTop", "roiRight", "roiBottom")); + assertThat(valueNames, hasItems("voltage", "rangeSigma", "sampleTime", "status", "rangingMode", "roiLeft", "roiTop", "roiRight", "roiBottom")); } assertFalse(simDeviceExists(deviceName)); } @Test public void testRange() { - withDevices((dev, sim) -> { - SimDouble range = sim.getDouble("range"); + withDevices((dev, rangeDeviceSim, ambientLightLevelDeviceSim) -> { + SimDouble range = rangeDeviceSim.getDouble("voltage"); assertNotNull(range); dev.getRange(); // Default is not specified but must not throw. @@ -68,8 +68,8 @@ public void testRange() { @Test public void testRangeSigma() { - withDevices((dev, sim) -> { - SimDouble rangeSigma = sim.getDouble("rangeSigma"); + withDevices((dev, rangeDeviceSim, ambientLightLevelDeviceSim) -> { + SimDouble rangeSigma = rangeDeviceSim.getDouble("rangeSigma"); assertNotNull(rangeSigma); dev.getRangeSigma(); // Default is not specified but must not throw. @@ -82,8 +82,8 @@ public void testRangeSigma() { @Test public void testStatus() { - withDevices((dev, sim) -> { - SimEnum status = sim.getEnum("status"); + withDevices((dev, rangeDeviceSim, ambientLightLevelDeviceSim) -> { + SimEnum status = rangeDeviceSim.getEnum("status"); assertNotNull(status); dev.getStatus(); // Default is not specified but must not throw. @@ -97,8 +97,8 @@ public void testStatus() { @Test public void testAmbientLightLevel() { - withDevices((dev, sim) -> { - SimDouble ambientLightLevel = sim.getDouble("ambientLightLevel"); + withDevices((dev, rangeDeviceSim, ambientLightLevelDeviceSim) -> { + SimDouble ambientLightLevel = ambientLightLevelDeviceSim.getDouble("voltage"); assertNotNull(ambientLightLevel); dev.getAmbientLightLevel(); // Default is not specified but must not throw. @@ -111,12 +111,12 @@ public void testAmbientLightLevel() { @Test public void testRangingMode() { - withDevices((dev, sim) -> { - SimDouble sampleTime = sim.getDouble("sampleTime"); + withDevices((dev, rangeDeviceSim, ambientLightLevelDeviceSim) -> { + SimDouble sampleTime = rangeDeviceSim.getDouble("sampleTime"); assertNotNull(sampleTime); assertThat(dev.getSampleTime(), is(24.0)); - SimEnum rangingMode = sim.getEnum("rangingMode"); + SimEnum rangingMode = rangeDeviceSim.getEnum("rangingMode"); assertNotNull(rangingMode); assertThat(dev.getRangingMode(), is(RangingMode.Short)); @@ -128,8 +128,8 @@ public void testRangingMode() { @Test public void testRangeOfInterest() { - withDevices((dev, sim) -> { - List roiList = Arrays.stream(new String[] {"Left", "Top", "Right", "Bottom"}).map(side -> sim.getInt("roi"+side)).toList(); + withDevices((dev, rangeDeviceSim, ambientLightLevelDeviceSim) -> { + List roiList = Arrays.stream(new String[] {"Left", "Top", "Right", "Bottom"}).map(side -> rangeDeviceSim.getInt("roi"+side)).toList(); assertThat(roiList, everyItem(notNullValue(SimInt.class))); assertEquals(0, roiList.get(0).get()); assertEquals(0, roiList.get(1).get()); @@ -156,21 +156,22 @@ private TimeOfFlight createDevice(int deviceId) { return MockedPlayingWithFusionTimeOfFlight.createMock(deviceId); } - private void withDevices(EncoderTest func) { + private void withDevices(DistanceSensorTest func) { withDevice(0, func); withDevice(1, func); withDevice(2, func); } - private void withDevice(int id, EncoderTest func) { + private void withDevice(int id, DistanceSensorTest func) { try(TimeOfFlight dev = createDevice(id)) { - SimDeviceSim sim = new SimDeviceSim("PlayingWithFusionTimeOfFlight", id); - func.test((TimeOfFlight)dev, sim); + SimDeviceSim rangeDeviceSim = new SimDeviceSim(String.format("CANAIn:PlayingWithFusionTimeOfFlight[%d]-rangeVoltsIsMM", id)); + SimDeviceSim ambientLightLevelDeviceSim = new SimDeviceSim(String.format("CANAIn:PlayingWithFusionTimeOfFlight[%d]-ambientLightLevelVoltsIsMcps", id)); + func.test(dev, rangeDeviceSim, ambientLightLevelDeviceSim); } } - private interface EncoderTest { - public void test(TimeOfFlight encoder, SimDeviceSim sim); + private interface DistanceSensorTest { + public void test(TimeOfFlight distanceSensor, SimDeviceSim rangeDeviceSim, SimDeviceSim ambientLightLevelDeviceSim); } } diff --git a/src/test/java/org/carlmontrobotics/lib199/testUtils/TestRules.java b/src/test/java/org/carlmontrobotics/lib199/testUtils/TestRules.java index cf0e991b..ef6d1199 100644 --- a/src/test/java/org/carlmontrobotics/lib199/testUtils/TestRules.java +++ b/src/test/java/org/carlmontrobotics/lib199/testUtils/TestRules.java @@ -1,5 +1,6 @@ package org.carlmontrobotics.lib199.testUtils; +import org.carlmontrobotics.lib199.Lib199Subsystem; import org.junit.rules.TestRule; import org.junit.runner.Description; import org.junit.runners.model.Statement; @@ -30,8 +31,13 @@ public Statement apply(Statement base, Description description) { return new Statement(){ @Override public void evaluate() throws Throwable { + // Ensure there are no async periodic things running before + // we reset the SimDeviceData so that they don't try to + // touch devices/data after they have been removed. + Lib199Subsystem.unregisterAllAsync(); SimDeviceSim.resetData(); base.evaluate(); + Lib199Subsystem.unregisterAllAsync(); SimDeviceSim.resetData(); } };