diff --git a/src/main/java/org/carlmontrobotics/lib199/MotorConfig.java b/src/main/java/org/carlmontrobotics/lib199/MotorConfig.java index f307b2b..0c12de1 100644 --- a/src/main/java/org/carlmontrobotics/lib199/MotorConfig.java +++ b/src/main/java/org/carlmontrobotics/lib199/MotorConfig.java @@ -5,6 +5,13 @@ public class MotorConfig { public static final MotorConfig NEO = new MotorConfig(70, 40); public static final MotorConfig NEO_550 = new MotorConfig(40, 20); + // The temp limit of 100C for the Vortex is based on the fact that its temp sensors are mounted directly on the + // windings (which is not the case for the Neo or Neo550, causing them to have very delayed temp readings) and the + // fact that the winding enamel will melt at 140C. + // See: https://www.chiefdelphi.com/t/rev-robotics-spark-flex-and-neo-vortex/442595/349?u=brettle + // As a result I think 100C should be safe. I wouldn't increase it past 120. --Dean + public static final MotorConfig NEO_VORTEX = new MotorConfig(100, 60); + public final int temperatureLimitCelsius, currentLimitAmps; public MotorConfig(int temperatureLimitCelsius, int currentLimitAmps) { diff --git a/src/main/java/org/carlmontrobotics/lib199/MotorControllerFactory.java b/src/main/java/org/carlmontrobotics/lib199/MotorControllerFactory.java index 8f3a11c..0cd62ee 100644 --- a/src/main/java/org/carlmontrobotics/lib199/MotorControllerFactory.java +++ b/src/main/java/org/carlmontrobotics/lib199/MotorControllerFactory.java @@ -14,9 +14,12 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkBase.ExternalFollower; import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkBase; +import com.revrobotics.CANSparkFlex; import com.revrobotics.CANSparkLowLevel; import com.revrobotics.SparkPIDController; +import org.carlmontrobotics.lib199.sim.MockSparkFlex; import org.carlmontrobotics.lib199.sim.MockSparkMax; import org.carlmontrobotics.lib199.sim.MockTalonSRX; import org.carlmontrobotics.lib199.sim.MockVictorSPX; @@ -120,7 +123,26 @@ public static CANSparkMax createSparkMax(int id, MotorConfig config) { spark = MockSparkMax.createMockSparkMax(id, CANSparkLowLevel.MotorType.kBrushless); } - MotorErrors.reportSparkMaxTemp(spark, config.temperatureLimitCelsius); + configureSpark(spark, config); + + return spark; + } + + public static CANSparkFlex createSparkFlex(int id, MotorConfig config) { + CANSparkFlex spark; + if (RobotBase.isReal()) { + spark = new CANSparkFlex(id, CANSparkLowLevel.MotorType.kBrushless); + } else { + spark = MockSparkFlex.createMockSparkFlex(id, CANSparkLowLevel.MotorType.kBrushless); + } + + configureSpark(spark, config); + + return spark; + } + + private static void configureSpark(CANSparkBase spark, MotorConfig config) { + MotorErrors.reportSparkTemp(spark, config.temperatureLimitCelsius); MotorErrors.reportError(spark.restoreFactoryDefaults()); //MotorErrors.reportError(spark.follow(ExternalFollower.kFollowerDisabled, 0)); @@ -128,7 +150,7 @@ public static CANSparkMax createSparkMax(int id, MotorConfig config) { MotorErrors.reportError(spark.enableVoltageCompensation(12)); MotorErrors.reportError(spark.setSmartCurrentLimit(config.currentLimitAmps)); - MotorErrors.checkSparkMaxErrors(spark); + MotorErrors.checkSparkErrors(spark); SparkPIDController controller = spark.getPIDController(); MotorErrors.reportError(controller.setOutputRange(-1, 1)); @@ -136,8 +158,6 @@ public static CANSparkMax createSparkMax(int id, MotorConfig config) { MotorErrors.reportError(controller.setI(0)); MotorErrors.reportError(controller.setD(0)); MotorErrors.reportError(controller.setFF(0)); - - return spark; } /** @@ -183,4 +203,4 @@ public static UsbCamera[] configureCameras(int numCameras) { for(int i = 0; i < numCameras; i++) cameras[i] = configureCamera(); return cameras; } -} \ No newline at end of file +} diff --git a/src/main/java/org/carlmontrobotics/lib199/MotorErrors.java b/src/main/java/org/carlmontrobotics/lib199/MotorErrors.java index 686fd8c..6cacbe1 100644 --- a/src/main/java/org/carlmontrobotics/lib199/MotorErrors.java +++ b/src/main/java/org/carlmontrobotics/lib199/MotorErrors.java @@ -4,6 +4,8 @@ import java.util.concurrent.ConcurrentHashMap; import com.ctre.phoenix.ErrorCode; +import com.revrobotics.CANSparkBase; +import com.revrobotics.CANSparkFlex; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkBase.FaultID; import com.revrobotics.REVLibError; @@ -12,17 +14,17 @@ public final class MotorErrors { - private static final ConcurrentHashMap temperatureSparks = new ConcurrentHashMap<>(); + private static final ConcurrentHashMap temperatureSparks = new ConcurrentHashMap<>(); private static final ConcurrentHashMap sparkTemperatureLimits = new ConcurrentHashMap<>(); private static final ConcurrentHashMap overheatedSparks = new ConcurrentHashMap<>(); - private static final ConcurrentHashMap flags = new ConcurrentHashMap<>(); - private static final ConcurrentHashMap stickyFlags = new ConcurrentHashMap<>(); + private static final ConcurrentHashMap flags = new ConcurrentHashMap<>(); + private static final ConcurrentHashMap stickyFlags = new ConcurrentHashMap<>(); public static final int kOverheatTripCount = 5; static { - Lib199Subsystem.registerAsyncPeriodic(MotorErrors::doReportSparkMaxTemp); - Lib199Subsystem.registerAsyncPeriodic(MotorErrors::printSparkMaxErrorMessages); + Lib199Subsystem.registerAsyncPeriodic(MotorErrors::doReportSparkTemp); + Lib199Subsystem.registerAsyncPeriodic(MotorErrors::printSparkErrorMessages); } public static void reportError(ErrorCode error) { @@ -55,7 +57,7 @@ private static > void reportError(String vendor, T error, T ok System.err.println(Arrays.toString(stack)); } - public static void checkSparkMaxErrors(CANSparkMax spark) { + public static void checkSparkErrors(CANSparkBase spark) { //Purposely obviously impersonal to differentiate from actual computer generated errors short faults = spark.getFaults(); short stickyFaults = spark.getStickyFaults(); @@ -63,17 +65,22 @@ public static void checkSparkMaxErrors(CANSparkMax spark) { short prevStickyFaults = stickyFlags.containsKey(spark) ? stickyFlags.get(spark) : 0; if (spark.getFaults() != 0 && prevFaults != faults) { - System.err.println("Whoops, big oopsie : fault error(s) with spark max id : " + spark.getDeviceId() + ": [ " + formatFaults(spark) + "], ooF!"); + System.err.println("Whoops, big oopsie : fault error(s) with spark id : " + spark.getDeviceId() + ": [ " + formatFaults(spark) + "], ooF!"); } if (spark.getStickyFaults() != 0 && prevStickyFaults != stickyFaults) { - System.err.println("Bruh, you did an Error : sticky fault(s) error with spark max id : " + spark.getDeviceId() + ": " + formatStickyFaults(spark) + ", Ouch!"); + System.err.println("Bruh, you did an Error : sticky fault(s) error with spark id : " + spark.getDeviceId() + ": " + formatStickyFaults(spark) + ", Ouch!"); } spark.clearFaults(); flags.put(spark, faults); stickyFlags.put(spark, stickyFaults); } - private static String formatFaults(CANSparkMax spark) { + @Deprecated + public static void checkSparkMaxErrors(CANSparkMax spark) { + checkSparkErrors((CANSparkBase)spark); + } + + private static String formatFaults(CANSparkBase spark) { String out = ""; for(FaultID fault: FaultID.values()) { if(spark.getFault(fault)) { @@ -83,7 +90,7 @@ private static String formatFaults(CANSparkMax spark) { return out; } - private static String formatStickyFaults(CANSparkMax spark) { + private static String formatStickyFaults(CANSparkBase spark) { String out = ""; for(FaultID fault: FaultID.values()) { if(spark.getStickyFault(fault)) { @@ -93,8 +100,13 @@ private static String formatStickyFaults(CANSparkMax spark) { return out; } + @Deprecated public static void printSparkMaxErrorMessages() { - flags.keySet().forEach((spark) -> checkSparkMaxErrors(spark)); + printSparkErrorMessages(); + } + + public static void printSparkErrorMessages() { + flags.keySet().forEach(MotorErrors::checkSparkErrors); } public static CANSparkMax createDummySparkMax() { @@ -105,26 +117,42 @@ public static CANSparkMax createDummySparkMax() { public static void reportSparkMaxTemp(CANSparkMax spark, TemperatureLimit temperatureLimit) { reportSparkMaxTemp(spark, temperatureLimit.limit); } - + public static boolean isSparkMaxOverheated(CANSparkMax spark){ int id = spark.getDeviceId(); int motorMaxTemp = sparkTemperatureLimits.get(id); return ( spark.getMotorTemperature() >= motorMaxTemp ); } + @Deprecated public static void reportSparkMaxTemp(CANSparkMax spark, int temperatureLimit) { + reportSparkTemp((CANSparkBase) spark, temperatureLimit); + } + + public static void reportSparkTemp(CANSparkBase spark, int temperatureLimit) { int id = spark.getDeviceId(); temperatureSparks.put(id, spark); sparkTemperatureLimits.put(id, temperatureLimit); overheatedSparks.put(id, 0); } + @Deprecated public static void doReportSparkMaxTemp() { + doReportSparkTemp(); + } + + public static void doReportSparkTemp() { temperatureSparks.forEach((port, spark) -> { double temp = spark.getMotorTemperature(); double limit = sparkTemperatureLimits.get(port); int numTrips = overheatedSparks.get(port); - SmartDashboard.putNumber("Port " + port + " Spark Max Temp", temp); + String sparkType = "of unknown type"; + if (spark instanceof CANSparkMax) { + sparkType = "Max"; + } else if (spark instanceof CANSparkFlex) { + sparkType = "Flex"; + } + SmartDashboard.putNumber(String.format("Port %d Spark %s Temp", port, sparkType), temp); if(numTrips < kOverheatTripCount) { if(temp > limit) { @@ -140,7 +168,7 @@ public static void doReportSparkMaxTemp() { // Set trip count to kOverheatTripCount + 1 to flag that an error message has already been printed // This prevents the error message from being re-printed every time the periodic method is run overheatedSparks.put(port, kOverheatTripCount + 1); - System.err.println("Port " + port + " spark max is operating at " + temp + " degrees Celsius! It will be disabled until the robot code is restarted."); + System.err.println("Port " + port + " spark is operating at " + temp + " degrees Celsius! It will be disabled until the robot code is restarted."); } spark.setSmartCurrentLimit(1); } @@ -160,4 +188,4 @@ private TemperatureLimit(int limit) { } } -} \ No newline at end of file +} diff --git a/src/main/java/org/carlmontrobotics/lib199/SensorFactory.java b/src/main/java/org/carlmontrobotics/lib199/SensorFactory.java index fae919d..2bcedc5 100644 --- a/src/main/java/org/carlmontrobotics/lib199/SensorFactory.java +++ b/src/main/java/org/carlmontrobotics/lib199/SensorFactory.java @@ -1,9 +1,10 @@ package org.carlmontrobotics.lib199; import org.carlmontrobotics.lib199.sim.MockedCANCoder; +import org.carlmontrobotics.lib199.sim.MockedPlayingWithFusionTimeOfFlight; import com.ctre.phoenix6.hardware.CANcoder; - +import com.playingwithfusion.TimeOfFlight; import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.cscore.UsbCamera; @@ -59,4 +60,13 @@ public static UsbCamera[] configureCameras(int numCameras) { return cameras; } + public static TimeOfFlight createPlayingWithFusionTimeOfFlight(int portNumber) { + TimeOfFlight tof; + if (RobotBase.isReal()) { + tof = new TimeOfFlight(portNumber); + } else { + tof = MockedPlayingWithFusionTimeOfFlight.createMock(portNumber); + } + return tof; + } } diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkBase.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkBase.java new file mode 100644 index 0000000..cc63577 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkBase.java @@ -0,0 +1,277 @@ +package org.carlmontrobotics.lib199.sim; + +import java.util.concurrent.ConcurrentHashMap; + +import org.carlmontrobotics.lib199.Lib199Subsystem; +import org.carlmontrobotics.lib199.Mocks; +import org.carlmontrobotics.lib199.REVLibErrorAnswer; + +import com.revrobotics.CANSparkBase; +import com.revrobotics.CANSparkBase.ExternalFollower; +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.REVLibError; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkAbsoluteEncoder; +import com.revrobotics.SparkMaxAlternateEncoder; +import com.revrobotics.SparkAnalogSensor; +import com.revrobotics.SparkPIDController; +import com.revrobotics.SparkRelativeEncoder; +import com.revrobotics.SparkRelativeEncoder.Type; + +import edu.wpi.first.hal.SimDevice; +import edu.wpi.first.wpilibj.motorcontrol.MotorController; + +/** + * An extension of {@link MockedMotorBase} which implements spark-max-specific functionality + */ +public class MockSparkBase extends MockedMotorBase { + + private static final ConcurrentHashMap controllers = new ConcurrentHashMap<>(); + + public final MotorType type; + private final MockedEncoder encoder; + private final SparkPIDController pidController; + private final MockedSparkMaxPIDController pidControllerImpl; + private SparkAbsoluteEncoder absoluteEncoder = null; + private MockedEncoder alternateEncoder = null; + 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. + * @param name the name of the type of controller ("SparkMax" or "SparkFlex") + */ + public MockSparkBase(int port, MotorType type, String name) { + super(name, port); + this.type = type; + + if(type == MotorType.kBrushless) { + encoder = new MockedEncoder(SimDevice.create(device.getName() + "_RelativeEncoder"), MockedEncoder.NEO_BUILTIN_ENCODER_CPR, false) { + @Override + public REVLibError setInverted(boolean inverted) { + System.err.println( + "(MockedEncoder) SparkRelativeEncoder cannot be inverted separately from the motor in brushless mode!"); + return REVLibError.kParamInvalid; + } + }; + } else { + encoder = new MockedEncoder(SimDevice.create(device.getName() + "_RelativeEncoder"), MockedEncoder.NEO_BUILTIN_ENCODER_CPR, false); + } + + pidControllerImpl = new MockedSparkMaxPIDController(this); + pidController = Mocks.createMock(SparkPIDController.class, pidControllerImpl, new REVLibErrorAnswer()); + pidController.setFeedbackDevice(encoder); + + controllers.put(port, this); + + Lib199Subsystem.registerAsyncSimulationPeriodic(this); + } + + @Override + public double getRequestedSpeed() { + return pidControllerImpl.calculate(getCurrentDraw()); + } + + /** + * @param port the port of the controller to search for + * @return Queries the simulated motor controller with the given port + */ + public static MockSparkBase getControllerWithId(int port) { + return controllers.get(port); + } + + @Override + public void set(double speed) { + speed *= voltageCompensationNominalVoltage / defaultNominalVoltage; + pidControllerImpl.setDutyCycle(speed); + } + + public REVLibError follow(CANSparkBase leader) { + return follow(leader, false); + } + + public REVLibError follow(CANSparkBase leader, boolean invert) { + pidControllerImpl.follow(leader, invert); // No need to lookup the spark max if we already have it + return REVLibError.kOk; + } + + public REVLibError follow(ExternalFollower leader, int deviceID) { + return follow(leader, deviceID, false); + } + + public REVLibError follow(ExternalFollower leader, int deviceID, boolean invert) { + MotorController controller = null; + // Because ExternalFollower does not implement equals, this could result in bugs if the user passes in a custom ExternalFollower object, + // but I think that it's unlikely and users should use the builtin definitions anyway + if(leader.equals(ExternalFollower.kFollowerDisabled)) { + pidControllerImpl.stopFollowing(); + } else { + if(leader.equals(ExternalFollower.kFollowerSpark)) { + controller = getControllerWithId(deviceID); + } else if(leader.equals(ExternalFollower.kFollowerPhoenix)) { + // controller = MockPhoenixController.getControllerWithId(deviceID); + } + if(controller == null) { + System.err.println("Error: Attempted to follow unknown motor controller: " + leader + " " + deviceID); + return REVLibError.kFollowConfigMismatch; + } + pidControllerImpl.follow(controller, invert); + } + return REVLibError.kOk; + } + + public boolean isFollower() { + return pidControllerImpl.isFollower(); + } + + public int getDeviceId() { + return port; + } + + public RelativeEncoder getEncoder() { + return encoder; + } + + public RelativeEncoder getEncoder(SparkRelativeEncoder.Type type, int countsPerRev) { + if(type != Type.kHallSensor) { + System.err.println("Error: MockSparkMax only supports hall effect encoders"); + return null; + } + return getEncoder(); + } + + @Override + public void setInverted(boolean inverted) { + super.setInverted(inverted); + + // Set the encoder inversion directly to avoid the error message + if(type == MotorType.kBrushless) encoder.inverted = inverted; + } + + public REVLibError enableVoltageCompensation(double nominalVoltage) { + super.doEnableVoltageCompensation(nominalVoltage); + return REVLibError.kOk; + } + + public REVLibError disableVoltageCompensation() { + super.doDisableVoltageCompensation(); + return REVLibError.kOk; + } + + public SparkPIDController getPIDController() { + return pidController; + } + + public double getAppliedOutput() { + // MockedMotorBase returns speed before rate limiting. + // The current output is the speed after rate limiting. + return (isInverted ? -1.0 : 1.0) * speed.get(); + } + + public double getBusVoltage() { + return defaultNominalVoltage; + } + + @Override + public void close() { + controllers.remove(port); + super.close(); + } + + /** + * Creates a simulated {@link SparkAbsoluteEncoder} linked to this simulated controller. + * After this method has been called once, its output is cached for future invocations. + * For this reason, the method is also {@code synchronized}. + * + * @param encoderType ignored + * @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); + absoluteEncoder = Mocks.createMock(SparkAbsoluteEncoder.class, absoluteEncoderImpl, new REVLibErrorAnswer()); + } + return absoluteEncoder; + } + + /** + * Creates a simulated alternate encoder linked to this simulated controller. + * After this method has been called once, its output is cached for future invocations. + * This means that only the first call to this method will set the CPR of the encoder. + * For this reason, the method is also {@code synchronized}. + * + * @param countsPerRev the CPR of the absolute encoder + * @return the simulated encoder + */ + public RelativeEncoder getAlternateEncoder(int countsPerRev) { + return getAlternateEncoder(SparkMaxAlternateEncoder.Type.kQuadrature, countsPerRev); + } + + /** + * Creates a simulated {@link SparkMaxAlternateEncoder} linked to this simulated controller. + * After this method has been called once, its output is cached for future invocations. + * For this reason, the method is also {@code synchronized}. + * + * @param encoderType ignored + * @return the simulated encoder + */ + public synchronized RelativeEncoder getAlternateEncoder(SparkMaxAlternateEncoder.Type encoderType, int countsPerRev) { + if(alternateEncoder == null) { + alternateEncoder = new MockedEncoder(SimDevice.create(device.getName() + "_AlternateEncoder"), countsPerRev, false); + } + return alternateEncoder; + } + + /** + * Creates a simulated {@link SparkAnalogSensor} linked to this simulated controller. + * After this method has been called once, its output is cached for future invocations. + * For this reason, the method is also {@code synchronized}. + * + * @param mode setting this to {@link SparkAnalogSensor.Mode#kAbsolute} makes the position relative to the position on startup. + * We will assume that this value is always zero, so this parameter has no effect. + * @return the simulated encoder + */ + 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); + analogSensor = Mocks.createMock(SparkAnalogSensor.class, analogSensorImpl, new REVLibErrorAnswer()); + } + return analogSensor; + } + + public double getClosedLoopRampRate() { + return getRampRateClosedLoop(); + } + + public double getOpenLoopRampRate() { + return getRampRateOpenLoop(); + } + + public REVLibError setClosedLoopRampRate(double secondsFromNeutralToFull) { + setRampRateClosedLoop(secondsFromNeutralToFull); + return REVLibError.kOk; + } + + public REVLibError setOpenLoopRampRate(double secondsFromNeutralToFull) { + setRampRateOpenLoop(secondsFromNeutralToFull); + return REVLibError.kOk; + } + + public REVLibError setIdleMode(IdleMode mode) { + super.setBrakeModeEnabled(mode == IdleMode.kBrake); + return REVLibError.kOk; + } + + @Override + public void disable() { + // CANSparkBase sets the motor speed to zero rather than actually disabling the motor + set(0); + } + +} diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkFlex.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkFlex.java new file mode 100644 index 0000000..0a7991a --- /dev/null +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkFlex.java @@ -0,0 +1,18 @@ +package org.carlmontrobotics.lib199.sim; + +import org.carlmontrobotics.lib199.DummySparkMaxAnswer; +import org.carlmontrobotics.lib199.Mocks; + +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkFlex; + +public class MockSparkFlex extends MockSparkBase { + + public MockSparkFlex(int port, MotorType type) { + super(port, type, "SparkFlex"); + } + + public static CANSparkFlex createMockSparkFlex(int portPWM, MotorType type) { + return Mocks.createMock(CANSparkFlex.class, new MockSparkFlex(portPWM, type), new DummySparkMaxAnswer()); + } +} diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java index 11e884a..55805d6 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java @@ -1,288 +1,18 @@ package org.carlmontrobotics.lib199.sim; -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; -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkBase.ExternalFollower; -import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.REVLibError; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkAbsoluteEncoder; -import com.revrobotics.SparkMaxAlternateEncoder; -import com.revrobotics.SparkAnalogSensor; -import com.revrobotics.SparkPIDController; -import com.revrobotics.SparkRelativeEncoder; -import com.revrobotics.SparkRelativeEncoder.Type; - -import edu.wpi.first.hal.SimDevice; -import edu.wpi.first.wpilibj.motorcontrol.MotorController; - -/** - * An extension of {@link MockedMotorBase} which implements spark-max-specific functionality - */ -public class MockSparkMax extends MockedMotorBase { - - private static final ConcurrentHashMap controllers = new ConcurrentHashMap<>(); +import com.revrobotics.CANSparkMax; - public final MotorType type; - private final MockedEncoder encoder; - private final SparkPIDController pidController; - private final MockedSparkMaxPIDController pidControllerImpl; - private SparkAbsoluteEncoder absoluteEncoder = null; - private MockedEncoder alternateEncoder = null; - private SparkAnalogSensor analogSensor = null; +public class MockSparkMax extends MockSparkBase { - /** - * 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. - */ public MockSparkMax(int port, MotorType type) { - super("SparkMax", port); - this.type = type; - - if(type == MotorType.kBrushless) { - encoder = new MockedEncoder(SimDevice.create(device.getName() + "_RelativeEncoder"), MockedEncoder.NEO_BUILTIN_ENCODER_CPR, false) { - @Override - public REVLibError setInverted(boolean inverted) { - System.err.println( - "(MockedEncoder) SparkRelativeEncoder cannot be inverted separately from the motor in brushless mode!"); - return REVLibError.kParamInvalid; - } - }; - } else { - encoder = new MockedEncoder(SimDevice.create(device.getName() + "_RelativeEncoder"), MockedEncoder.NEO_BUILTIN_ENCODER_CPR, false); - } - - pidControllerImpl = new MockedSparkMaxPIDController(this); - pidController = Mocks.createMock(SparkPIDController.class, pidControllerImpl, new REVLibErrorAnswer()); - pidController.setFeedbackDevice(encoder); - - controllers.put(port, this); - - Lib199Subsystem.registerAsyncSimulationPeriodic(this); - } - - @Override - public double getRequestedSpeed() { - return pidControllerImpl.calculate(getCurrentDraw()); - } - - /** - * @param port the port of the controller to search for - * @return Queries the simulated motor controller with the given port - */ - public static MockSparkMax getControllerWithId(int port) { - return controllers.get(port); - } - - /** - * Creates a simulated {@link CANSparkMax} with an instance of this class acting as the underling implementation, and forwarding all unimplemented method calls to {@link DummySparkMaxAnswer} - * @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. - * @return the simulated {@link CANSparkMax} - */ - public static CANSparkMax createMockSparkMax(int port, MotorType type) { - return Mocks.createMock(CANSparkMax.class, new MockSparkMax(port, type), new DummySparkMaxAnswer()); - } - - @Override - public void set(double speed) { - speed *= voltageCompensationNominalVoltage / defaultNominalVoltage; - pidControllerImpl.setDutyCycle(speed); - } - - public REVLibError follow(CANSparkMax leader) { - return follow(leader, false); - } - - public REVLibError follow(CANSparkMax leader, boolean invert) { - pidControllerImpl.follow(leader, invert); // No need to lookup the spark max if we already have it - return REVLibError.kOk; - } - - public REVLibError follow(ExternalFollower leader, int deviceID) { - return follow(leader, deviceID, false); - } - - public REVLibError follow(ExternalFollower leader, int deviceID, boolean invert) { - MotorController controller = null; - // Because ExternalFollower does not implement equals, this could result in bugs if the user passes in a custom ExternalFollower object, - // but I think that it's unlikely and users should use the builtin definitions anyway - if(leader.equals(ExternalFollower.kFollowerDisabled)) { - pidControllerImpl.stopFollowing(); - } else { - if(leader.equals(ExternalFollower.kFollowerSpark)) { - controller = getControllerWithId(deviceID); - } else if(leader.equals(ExternalFollower.kFollowerPhoenix)) { - // controller = MockPhoenixController.getControllerWithId(deviceID); - } - if(controller == null) { - System.err.println("Error: Attempted to follow unknown motor controller: " + leader + " " + deviceID); - return REVLibError.kFollowConfigMismatch; - } - pidControllerImpl.follow(controller, invert); - } - return REVLibError.kOk; - } - - public boolean isFollower() { - return pidControllerImpl.isFollower(); - } - - public int getDeviceId() { - return port; - } - - public RelativeEncoder getEncoder() { - return encoder; - } - - public RelativeEncoder getEncoder(SparkRelativeEncoder.Type type, int countsPerRev) { - if(type != Type.kHallSensor) { - System.err.println("Error: MockSparkMax only supports hall effect encoders"); - return null; - } - return getEncoder(); + super(port, type, "SparkMax"); } - @Override - public void setInverted(boolean inverted) { - super.setInverted(inverted); - - // Set the encoder inversion directly to avoid the error message - if(type == MotorType.kBrushless) encoder.inverted = inverted; + public static CANSparkMax createMockSparkMax(int portPWM, MotorType type) { + return Mocks.createMock(CANSparkMax.class, new MockSparkMax(portPWM, type), new DummySparkMaxAnswer()); } - - public REVLibError enableVoltageCompensation(double nominalVoltage) { - super.doEnableVoltageCompensation(nominalVoltage); - return REVLibError.kOk; - } - - public REVLibError disableVoltageCompensation() { - super.doDisableVoltageCompensation(); - return REVLibError.kOk; - } - - public SparkPIDController getPIDController() { - return pidController; - } - - public double getAppliedOutput() { - // MockedMotorBase returns speed before rate limiting. - // The current output is the speed after rate limiting. - return (isInverted ? -1.0 : 1.0) * speed.get(); - } - - public double getBusVoltage() { - return defaultNominalVoltage; - } - - @Override - public void close() { - controllers.remove(port); - super.close(); - } - - /** - * Creates a simulated {@link SparkAbsoluteEncoder} linked to this simulated controller. - * After this method has been called once, its output is cached for future invocations. - * For this reason, the method is also {@code synchronized}. - * - * @param encoderType ignored - * @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); - absoluteEncoder = Mocks.createMock(SparkAbsoluteEncoder.class, absoluteEncoderImpl, new REVLibErrorAnswer()); - } - return absoluteEncoder; - } - - /** - * Creates a simulated alternate encoder linked to this simulated controller. - * After this method has been called once, its output is cached for future invocations. - * This means that only the first call to this method will set the CPR of the encoder. - * For this reason, the method is also {@code synchronized}. - * - * @param countsPerRev the CPR of the absolute encoder - * @return the simulated encoder - */ - public RelativeEncoder getAlternateEncoder(int countsPerRev) { - return getAlternateEncoder(SparkMaxAlternateEncoder.Type.kQuadrature, countsPerRev); - } - - /** - * Creates a simulated {@link SparkMaxAlternateEncoder} linked to this simulated controller. - * After this method has been called once, its output is cached for future invocations. - * For this reason, the method is also {@code synchronized}. - * - * @param encoderType ignored - * @return the simulated encoder - */ - public synchronized RelativeEncoder getAlternateEncoder(SparkMaxAlternateEncoder.Type encoderType, int countsPerRev) { - if(alternateEncoder == null) { - alternateEncoder = new MockedEncoder(SimDevice.create(device.getName() + "_AlternateEncoder"), countsPerRev, false); - } - return alternateEncoder; - } - - /** - * Creates a simulated {@link SparkAnalogSensor} linked to this simulated controller. - * After this method has been called once, its output is cached for future invocations. - * For this reason, the method is also {@code synchronized}. - * - * @param mode setting this to {@link SparkAnalogSensor.Mode#kAbsolute} makes the position relative to the position on startup. - * We will assume that this value is always zero, so this parameter has no effect. - * @return the simulated encoder - */ - 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); - analogSensor = Mocks.createMock(SparkAnalogSensor.class, analogSensorImpl, new REVLibErrorAnswer()); - } - return analogSensor; - } - - public double getClosedLoopRampRate() { - return getRampRateClosedLoop(); - } - - public double getOpenLoopRampRate() { - return getRampRateOpenLoop(); - } - - public REVLibError setClosedLoopRampRate(double secondsFromNeutralToFull) { - setRampRateClosedLoop(secondsFromNeutralToFull); - return REVLibError.kOk; - } - - public REVLibError setOpenLoopRampRate(double secondsFromNeutralToFull) { - setRampRateOpenLoop(secondsFromNeutralToFull); - return REVLibError.kOk; - } - - public REVLibError setIdleMode(IdleMode mode) { - super.setBrakeModeEnabled(mode == IdleMode.kBrake); - return REVLibError.kOk; - } - - @Override - public void disable() { - // CANSparkMax sets the motor speed to zero rather than actually disabling the motor - set(0); - } - } diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockedPlayingWithFusionTimeOfFlight.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockedPlayingWithFusionTimeOfFlight.java new file mode 100644 index 0000000..58e9597 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockedPlayingWithFusionTimeOfFlight.java @@ -0,0 +1,101 @@ +package org.carlmontrobotics.lib199.sim; + +import java.util.Arrays; + +import org.carlmontrobotics.lib199.Mocks; + +import com.playingwithfusion.TimeOfFlight.Status; +import com.playingwithfusion.TimeOfFlight; +import com.playingwithfusion.TimeOfFlight.RangingMode; + +import edu.wpi.first.hal.SimDevice; +import edu.wpi.first.hal.SimDevice.Direction; +import edu.wpi.first.hal.SimDouble; +import edu.wpi.first.hal.SimEnum; +import edu.wpi.first.hal.SimInt; + +public class MockedPlayingWithFusionTimeOfFlight implements AutoCloseable { + + private int port; + private SimDevice device; + private SimDouble range, rangeSigma, sampleTime, ambientLightLevel; + 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); + + // 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); + + String[] statusNames = Arrays.stream(Status.values()).map(Status::name).toArray(String[]::new); + status = device.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()); + + 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); + } + + public static TimeOfFlight createMock(int portNumber) { + return Mocks.createMock(TimeOfFlight.class, new MockedPlayingWithFusionTimeOfFlight(portNumber)); + } + + public double getAmbientLightLevel() { + return ambientLightLevel.get(); + } + + public double getRange() { + return range.get(); + } + + public double getRangeSigma() { + return rangeSigma.get(); + } + + public RangingMode getRangingMode() { + return RangingMode.values()[rangingMode.get()]; + } + + public double getSampleTime() { + return sampleTime.get(); + } + + public Status getStatus() { + return Status.values()[status.get()]; + } + + public boolean isRangeValid() { + return getStatus() == Status.Valid; + } + + public double pidGet() { + return getRange(); + } + + public void setRangeOfInterest(int topLeftX, int topLeftY, int bottomRightX, int bottomRightY) { + roiLeft.set(topLeftX); + roiTop.set(topLeftY); + roiRight.set(bottomRightX); + roiBottom.set(bottomRightY); + } + + public void setRangingMode(RangingMode newMode, double newSampleTime) { + rangingMode.set(newMode.ordinal()); + sampleTime.set(newSampleTime); + } + + @Override + public void close() { + device.close(); + } +} diff --git a/src/test/java/org/carlmontrobotics/lib199/SensorFactoryTest.java b/src/test/java/org/carlmontrobotics/lib199/SensorFactoryTest.java new file mode 100644 index 0000000..2625198 --- /dev/null +++ b/src/test/java/org/carlmontrobotics/lib199/SensorFactoryTest.java @@ -0,0 +1,40 @@ +package org.carlmontrobotics.lib199; + +import static org.junit.Assert.assertEquals; +import static org.junit.Assert.assertTrue; +import static org.mockito.Mockito.mockingDetails; + +import org.carlmontrobotics.lib199.testUtils.ErrStreamTest; +import org.carlmontrobotics.lib199.testUtils.TestRules; +import org.junit.ClassRule; +import org.junit.Rule; +import org.junit.Test; + +import com.playingwithfusion.TimeOfFlight; + +public class SensorFactoryTest extends ErrStreamTest { + + @ClassRule + public static TestRules.InitializeHAL classRule = new TestRules.InitializeHAL(); + @Rule + public TestRules.ResetSimDeviceSimData testRule = new TestRules.ResetSimDeviceSimData(); + + @Test + // AutoClosable.close() throws Exception + public void testCreateNoErrors() throws Exception { + // Call close to free PWM ports + ((AutoCloseable)SensorFactory.createCANCoder(0)).close(); + ((AutoCloseable)SensorFactory.createCANCoder(1)).close(); + ((AutoCloseable)SensorFactory.createPlayingWithFusionTimeOfFlight(0)).close(); + ((AutoCloseable)SensorFactory.createPlayingWithFusionTimeOfFlight(1)).close(); + assertEquals(0, errStream.toByteArray().length); + } + + @Test + public void testPlayingWithFusionTimeOfFlightIsMock() { + // Call close to free PWM ports + try (TimeOfFlight dev = SensorFactory.createPlayingWithFusionTimeOfFlight(0)) { + assertTrue(mockingDetails(dev).isMock()); + } + } +} diff --git a/src/test/java/org/carlmontrobotics/lib199/sim/MockSparkMaxTest.java b/src/test/java/org/carlmontrobotics/lib199/sim/MockSparkMaxTest.java index 0745f07..e4e0d07 100644 --- a/src/test/java/org/carlmontrobotics/lib199/sim/MockSparkMaxTest.java +++ b/src/test/java/org/carlmontrobotics/lib199/sim/MockSparkMaxTest.java @@ -1,33 +1,22 @@ 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(); + public static TestRules.InitializeHAL simClassRule = new TestRules.InitializeHAL(); @Rule - public TestRules.ResetSimDeviceSimData simTestRule = new TestRules.ResetSimDeviceSimData(); + public TestRules.ResetSimDeviceSimData simTestRule = new TestRules.ResetSimDeviceSimData(); @Test public void testHasEncoder() { diff --git a/src/test/java/org/carlmontrobotics/lib199/sim/MockedPlayingWithFusionTimeOfFlightTest.java b/src/test/java/org/carlmontrobotics/lib199/sim/MockedPlayingWithFusionTimeOfFlightTest.java new file mode 100644 index 0000000..82e8427 --- /dev/null +++ b/src/test/java/org/carlmontrobotics/lib199/sim/MockedPlayingWithFusionTimeOfFlightTest.java @@ -0,0 +1,176 @@ +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 static org.hamcrest.MatcherAssert.*; +import static org.hamcrest.CoreMatchers.*; + +import java.util.Arrays; +import java.util.List; +import java.util.stream.Stream; + +import com.playingwithfusion.TimeOfFlight; +import com.playingwithfusion.TimeOfFlight.Status; +import com.playingwithfusion.TimeOfFlight.RangingMode; + +import org.carlmontrobotics.lib199.testUtils.TestRules; +import org.junit.ClassRule; +import org.junit.Rule; +import org.junit.Test; + +import edu.wpi.first.hal.SimDouble; +import edu.wpi.first.hal.SimEnum; +import edu.wpi.first.hal.SimInt; +import edu.wpi.first.wpilibj.simulation.SimDeviceSim; + +public class MockedPlayingWithFusionTimeOfFlightTest { + + @ClassRule + public static TestRules.InitializeHAL classRule = new TestRules.InitializeHAL(); + @Rule + public TestRules.ResetSimDeviceSimData testRule = new TestRules.ResetSimDeviceSimData(); + + @Test + public void testDeviceCreation() { + assertTestDeviceCreation(0); + assertTestDeviceCreation(1); + assertTestDeviceCreation(2); + } + + private void assertTestDeviceCreation(int id) { + String deviceName = String.format("PlayingWithFusionTimeOfFlight[%d]", id); + assertFalse(simDeviceExists(deviceName)); + try(TimeOfFlight dev = createDevice(id)) { + assertTrue(simDeviceExists(deviceName)); + SimDeviceSim sim = new SimDeviceSim("PlayingWithFusionTimeOfFlight", id); + var valueNames = Stream.of(sim.enumerateValues()).map(info -> info.name).toList(); + assertThat(valueNames, hasItems("range", "rangeSigma", "sampleTime", "ambientLightLevel", "status", "rangingMode", "roiLeft", "roiTop", "roiRight", "roiBottom")); + } + assertFalse(simDeviceExists(deviceName)); + } + + @Test + public void testRange() { + withDevices((dev, sim) -> { + SimDouble range = sim.getDouble("range"); + assertNotNull(range); + dev.getRange(); // Default is not specified but must not throw. + + for (double val : new double[] {15.0, 150.0, 1000.0}) { + range.set(val); + assertThat(dev.getRange(), is(val)); + assertThat(dev.pidGet(), is(val)); + } + }); + } + + @Test + public void testRangeSigma() { + withDevices((dev, sim) -> { + SimDouble rangeSigma = sim.getDouble("rangeSigma"); + assertNotNull(rangeSigma); + dev.getRangeSigma(); // Default is not specified but must not throw. + + for (double val : new double[] {15.0, 150.0, 1000.0}) { + rangeSigma.set(val); + assertThat(dev.getRangeSigma(), is(val)); + } + }); + } + + @Test + public void testStatus() { + withDevices((dev, sim) -> { + SimEnum status = sim.getEnum("status"); + assertNotNull(status); + dev.getStatus(); // Default is not specified but must not throw. + + for (Status val : Status.values()) { + status.set(val.ordinal()); + assertThat(dev.getStatus(), is(val)); + assertThat(dev.isRangeValid(), is(val == Status.Valid)); + } + }); + } + + @Test + public void testAmbientLightLevel() { + withDevices((dev, sim) -> { + SimDouble ambientLightLevel = sim.getDouble("ambientLightLevel"); + assertNotNull(ambientLightLevel); + dev.getAmbientLightLevel(); // Default is not specified but must not throw. + + for (double val : new double[] {15.0, 150.0, 1000.0}) { + ambientLightLevel.set(val); + assertThat(dev.getAmbientLightLevel(), is(val)); + } + }); + } + + @Test + public void testRangingMode() { + withDevices((dev, sim) -> { + SimDouble sampleTime = sim.getDouble("sampleTime"); + assertNotNull(sampleTime); + assertThat(dev.getSampleTime(), is(24.0)); + + SimEnum rangingMode = sim.getEnum("rangingMode"); + assertNotNull(rangingMode); + assertThat(dev.getRangingMode(), is(RangingMode.Short)); + + dev.setRangingMode(RangingMode.Medium, 100.0); + assertThat(dev.getRangingMode(), is(RangingMode.Medium)); + assertThat(dev.getSampleTime(), is(100.0)); + }); + } + + @Test + public void testRangeOfInterest() { + withDevices((dev, sim) -> { + List roiList = Arrays.stream(new String[] {"Left", "Top", "Right", "Bottom"}).map(side -> sim.getInt("roi"+side)).toList(); + assertThat(roiList, everyItem(notNullValue(SimInt.class))); + assertEquals(0, roiList.get(0).get()); + assertEquals(0, roiList.get(1).get()); + assertEquals(15, roiList.get(2).get()); + assertEquals(15, roiList.get(3).get()); + + dev.setRangeOfInterest(1, 2, 5, 6); + assertEquals(1, roiList.get(0).get()); + assertEquals(2, roiList.get(1).get()); + assertEquals(5, roiList.get(2).get()); + assertEquals(6, roiList.get(3).get()); + }); + } + + private boolean simDeviceExists(String deviceName) { + return Stream.of(SimDeviceSim.enumerateDevices(deviceName)) + .map(info -> info.name) + .distinct() + .filter(name -> name.equals(deviceName)) + .count() == 1; + } + + private TimeOfFlight createDevice(int deviceId) { + return MockedPlayingWithFusionTimeOfFlight.createMock(deviceId); + } + + private void withDevices(EncoderTest func) { + withDevice(0, func); + withDevice(1, func); + withDevice(2, func); + } + + private void withDevice(int id, EncoderTest func) { + try(TimeOfFlight dev = createDevice(id)) { + SimDeviceSim sim = new SimDeviceSim("PlayingWithFusionTimeOfFlight", id); + func.test((TimeOfFlight)dev, sim); + } + } + + private interface EncoderTest { + public void test(TimeOfFlight encoder, SimDeviceSim sim); + } + +} diff --git a/src/test/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDControllerTest.java b/src/test/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDControllerTest.java index c53bdcc..b958dbd 100644 --- a/src/test/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDControllerTest.java +++ b/src/test/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDControllerTest.java @@ -15,7 +15,7 @@ import org.junit.Test; public class MockedSparkMaxPIDControllerTest { - + @Test public void testResponses() { MockSparkMax mockSparkMax = new MockSparkMax(0, MotorType.kBrushless);