From 9eb3b3ecd8bd7b62c18578ce2d1b5ff39a1dd63c Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Tue, 2 Apr 2024 16:51:52 -0700 Subject: [PATCH 1/9] Add MockedSparkAbsoluteEncoder. --- .../lib199/sim/MockSparkMax.java | 3 + .../sim/MockedSparkAbsoluteEncoder.java | 84 +++++++++++++++++++ 2 files changed, 87 insertions(+) create mode 100644 src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkAbsoluteEncoder.java diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java index 7a1f6aa5..ce4fa387 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java @@ -7,6 +7,7 @@ import org.carlmontrobotics.lib199.Mocks; import org.carlmontrobotics.lib199.REVLibErrorAnswer; +import com.revrobotics.AbsoluteEncoder; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkBase.ExternalFollower; import com.revrobotics.CANSparkBase.IdleMode; @@ -27,6 +28,7 @@ public class MockSparkMax { private final SimDevice motor; private final SimDouble speed; private RelativeEncoder encoder; + private AbsoluteEncoder absoluteEncoder; private SparkPIDController pidController; private boolean isInverted; // Since we need to keep a record of all the motor's followers @@ -37,6 +39,7 @@ public MockSparkMax(int port, MotorType type) { motor = SimDevice.create("SparkMax", port); speed = motor.createDouble("Motor Output", Direction.kOutput, 0); encoder = Mocks.createMock(RelativeEncoder.class, new MockedSparkEncoder(port), new REVLibErrorAnswer()); + absoluteEncoder = Mocks.createMock(AbsoluteEncoder.class, new MockedSparkAbsoluteEncoder(port), new REVLibErrorAnswer()); pidController = Mocks.createMock(SparkPIDController.class, new MockedSparkMaxPIDController(), new REVLibErrorAnswer()); isInverted = false; } diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkAbsoluteEncoder.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkAbsoluteEncoder.java new file mode 100644 index 00000000..febec445 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkAbsoluteEncoder.java @@ -0,0 +1,84 @@ +package org.carlmontrobotics.lib199.sim; + +import java.util.HashMap; + +import org.carlmontrobotics.lib199.Lib199Subsystem; + +import com.revrobotics.REVLibError; + +import edu.wpi.first.hal.SimDevice; +import edu.wpi.first.hal.SimDouble; +import edu.wpi.first.hal.SimDevice.Direction; + +public class MockedSparkAbsoluteEncoder implements AutoCloseable, Runnable { + + private static final HashMap sims = new HashMap<>(); + + private SimDevice device; + private SimDouble rotations; + private double velocity; + private double positionConversionFactor = 1; + private double velocityConversionFactor = 1; + private double lastRotations = 0; + private double lastTime = 0; + private double zeroOffset = 0.0; + + public MockedSparkAbsoluteEncoder(int id) { + device = SimDevice.create("AbsoluteEncoder", id); + rotations = device.createDouble("rotations", Direction.kInput, 0); + sims.put(id, this); + Lib199Subsystem.registerAsyncPeriodic(this); + } + + public double getPosition() { + return positionConversionFactor * rotations.get() - zeroOffset; + } + + public REVLibError setPositionConversionFactor(double positionConversionFactor) { + this.positionConversionFactor = positionConversionFactor; + return REVLibError.kOk; + } + + public REVLibError setZeroOffset(double zeroOffset) { + this.zeroOffset = zeroOffset; + return REVLibError.kOk; + } + + public double getZeroOffset() { + return zeroOffset; + } + + public double getPositionConversionFactor() { + return positionConversionFactor; + } + + public double getVelocity() { + return velocity; + } + + public REVLibError setVelocityConversionFactor(double velocityConversionFactor) { + this.velocityConversionFactor = velocityConversionFactor; + return REVLibError.kOk; + } + + public double getVelocityConversionFactor() { + return velocityConversionFactor; + } + + @Override + public void run() { + double t = System.currentTimeMillis() / 1000D; + double dt = t - lastTime; + double curRotations = rotations.get(); + double dCount = curRotations - lastRotations; + lastTime = t; + lastRotations = curRotations; + double newVelocity = velocityConversionFactor * ( dCount / dt ) * 60; + velocity = Double.isNaN(newVelocity) ? 0 : newVelocity; + } + + @Override + public void close() { + device.close(); + } +} \ No newline at end of file From 0ce129844b9b918dad0207c0e1020fc26588f461 Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Tue, 2 Apr 2024 17:33:07 -0700 Subject: [PATCH 2/9] Add methods that return SparkAbsoluteEncoder. --- .../lib199/sim/MockSparkMax.java | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java index ce4fa387..2e79ab89 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java @@ -14,6 +14,8 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.REVLibError; import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkAbsoluteEncoder; +import com.revrobotics.SparkMaxAbsoluteEncoder; import com.revrobotics.SparkPIDController; import edu.wpi.first.hal.SimDevice; @@ -28,7 +30,7 @@ public class MockSparkMax { private final SimDevice motor; private final SimDouble speed; private RelativeEncoder encoder; - private AbsoluteEncoder absoluteEncoder; + private SparkAbsoluteEncoder absoluteEncoder; private SparkPIDController pidController; private boolean isInverted; // Since we need to keep a record of all the motor's followers @@ -39,7 +41,7 @@ public MockSparkMax(int port, MotorType type) { motor = SimDevice.create("SparkMax", port); speed = motor.createDouble("Motor Output", Direction.kOutput, 0); encoder = Mocks.createMock(RelativeEncoder.class, new MockedSparkEncoder(port), new REVLibErrorAnswer()); - absoluteEncoder = Mocks.createMock(AbsoluteEncoder.class, new MockedSparkAbsoluteEncoder(port), new REVLibErrorAnswer()); + absoluteEncoder = Mocks.createMock(SparkAbsoluteEncoder.class, new MockedSparkAbsoluteEncoder(port), new REVLibErrorAnswer()); pidController = Mocks.createMock(SparkPIDController.class, new MockedSparkMaxPIDController(), new REVLibErrorAnswer()); isInverted = false; } @@ -88,6 +90,18 @@ public RelativeEncoder getEncoder() { return encoder; } + public SparkAbsoluteEncoder getAbsoluteEncoder() { + return absoluteEncoder; + } + + public SparkAbsoluteEncoder getAbsoluteEncoder(SparkMaxAbsoluteEncoder.Type encoderType) { + return absoluteEncoder; + } + + public SparkAbsoluteEncoder getAbsoluteEncoder(SparkAbsoluteEncoder.Type encoderType) { + return absoluteEncoder; + } + public void setInverted(boolean inverted) { isInverted = inverted; } From 9dc6ffcd16058ea84a0067a88a61a03850405cba Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Thu, 4 Apr 2024 18:35:04 -0700 Subject: [PATCH 3/9] Add sim support for Spark Flex controllers and Vortex motors. Refactor common Spark functionality into MockSparkBase. Use CANSparkBase instead of CANSparkMax where appropriate. Add methods without Max or Flex in the name and deprecate most methods with Max in the name. Add a MotorConfig for the Vortex. --- .../carlmontrobotics/lib199/MotorConfig.java | 7 + .../lib199/MotorControllerFactory.java | 29 +++- .../carlmontrobotics/lib199/MotorErrors.java | 54 +++++-- .../lib199/sim/MockSparkBase.java | 136 ++++++++++++++++++ .../lib199/sim/MockSparkFlex.java | 18 +++ .../lib199/sim/MockSparkMax.java | 130 +---------------- 6 files changed, 231 insertions(+), 143 deletions(-) create mode 100644 src/main/java/org/carlmontrobotics/lib199/sim/MockSparkBase.java create mode 100644 src/main/java/org/carlmontrobotics/lib199/sim/MockSparkFlex.java diff --git a/src/main/java/org/carlmontrobotics/lib199/MotorConfig.java b/src/main/java/org/carlmontrobotics/lib199/MotorConfig.java index f307b2bd..0c12de1a 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 8f3a11c9..531de428 100644 --- a/src/main/java/org/carlmontrobotics/lib199/MotorControllerFactory.java +++ b/src/main/java/org/carlmontrobotics/lib199/MotorControllerFactory.java @@ -14,9 +14,13 @@ 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.MockSparkBase; +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 +124,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 +151,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 +159,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; } /** diff --git a/src/main/java/org/carlmontrobotics/lib199/MotorErrors.java b/src/main/java/org/carlmontrobotics/lib199/MotorErrors.java index 7e8249f4..05f75102 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((spark) -> checkSparkErrors(spark)); } public static CANSparkMax createDummySparkMax() { @@ -106,19 +118,35 @@ public static void reportSparkMaxTemp(CANSparkMax spark, TemperatureLimit temper reportSparkMaxTemp(spark, temperatureLimit.limit); } + @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) { @@ -134,7 +162,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); } 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 00000000..8bf30d5e --- /dev/null +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkBase.java @@ -0,0 +1,136 @@ +package org.carlmontrobotics.lib199.sim; + +import java.util.concurrent.ConcurrentHashMap; +import java.util.concurrent.CopyOnWriteArrayList; + +import org.carlmontrobotics.lib199.DummySparkMaxAnswer; +import org.carlmontrobotics.lib199.Mocks; +import org.carlmontrobotics.lib199.REVLibErrorAnswer; + +import com.revrobotics.AbsoluteEncoder; +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.SparkMaxAbsoluteEncoder; +import com.revrobotics.SparkPIDController; + +import edu.wpi.first.hal.SimDevice; +import edu.wpi.first.hal.SimDevice.Direction; +import edu.wpi.first.hal.SimDouble; + +public class MockSparkBase { + // Assign the CAN port to a PWM port so it works with the simulator. Not a fan + // of this solution though + // CAN ports should be separate from PWM ports + private final int port; + private final SimDevice motor; + private final SimDouble speed; + private RelativeEncoder encoder; + private SparkAbsoluteEncoder absoluteEncoder; + private SparkPIDController pidController; + private boolean isInverted; + // Since we need to keep a record of all the motor's followers + private static ConcurrentHashMap> followMap = new ConcurrentHashMap<>(); + + public MockSparkBase(int port, MotorType type, String simDeviceName) { + this.port = port; + motor = SimDevice.create(simDeviceName, port); + speed = motor.createDouble("Motor Output", Direction.kOutput, 0); + encoder = Mocks.createMock(RelativeEncoder.class, new MockedSparkEncoder(port), new REVLibErrorAnswer()); + absoluteEncoder = Mocks.createMock(SparkAbsoluteEncoder.class, new MockedSparkAbsoluteEncoder(port), new REVLibErrorAnswer()); + pidController = Mocks.createMock(SparkPIDController.class, new MockedSparkMaxPIDController(), new REVLibErrorAnswer()); + isInverted = false; + } + + public void set(double speed) { + speed = (isInverted ? -1.0 : 1.0) * speed; + this.speed.set(speed); + if (followMap.containsKey(getDeviceId())) { + for (SimDouble motorOutput : followMap.get(getDeviceId())) motorOutput.set(speed); + } + } + + public REVLibError follow(CANSparkMax leader) { + return follow(leader, false); + } + + public REVLibError follow(CANSparkMax leader, boolean invert) { + return follow(ExternalFollower.kFollowerSpark, leader.getDeviceId(), invert); + } + + public REVLibError follow(ExternalFollower leader, int deviceID) { + return follow(leader, deviceID, false); + } + + public REVLibError follow(ExternalFollower leader, int deviceID, boolean invert) { + if (!followMap.containsKey(deviceID)) { + followMap.put(deviceID, new CopyOnWriteArrayList()); + } + followMap.get(deviceID).add(speed); + return REVLibError.kOk; + } + + public double get() { + return speed.get(); + } + + public int getDeviceId() { + return port; + } + + public RelativeEncoder getEncoder() { + return encoder; + } + + public SparkAbsoluteEncoder getAbsoluteEncoder() { + return absoluteEncoder; + } + + public SparkAbsoluteEncoder getAbsoluteEncoder(SparkMaxAbsoluteEncoder.Type encoderType) { + return absoluteEncoder; + } + + public SparkAbsoluteEncoder getAbsoluteEncoder(SparkAbsoluteEncoder.Type encoderType) { + return absoluteEncoder; + } + + public void setInverted(boolean inverted) { + isInverted = inverted; + } + + public boolean getInverted() { + return isInverted; + } + + public REVLibError restoreFactoryDefaults() { + return REVLibError.kOk; + } + + public REVLibError setIdleMode(IdleMode mode) { + return REVLibError.kOk; + } + + public REVLibError enableVoltageCompensation(double nominalVoltage) { + return REVLibError.kOk; + } + + public REVLibError disableVoltageCompensation() { + return REVLibError.kOk; + } + + public REVLibError setSmartCurrentLimit(int limit) { + return REVLibError.kOk; + } + + public SparkPIDController getPIDController() { + return pidController; + } + + public void setVoltage(double outputVolts) { + set(outputVolts / 12); + } +} \ No newline at end of file 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 00000000..0a7991a3 --- /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 2e79ab89..55805d61 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java @@ -1,140 +1,18 @@ package org.carlmontrobotics.lib199.sim; -import java.util.concurrent.ConcurrentHashMap; -import java.util.concurrent.CopyOnWriteArrayList; - import org.carlmontrobotics.lib199.DummySparkMaxAnswer; import org.carlmontrobotics.lib199.Mocks; -import org.carlmontrobotics.lib199.REVLibErrorAnswer; -import com.revrobotics.AbsoluteEncoder; -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.SparkMaxAbsoluteEncoder; -import com.revrobotics.SparkPIDController; - -import edu.wpi.first.hal.SimDevice; -import edu.wpi.first.hal.SimDevice.Direction; -import edu.wpi.first.hal.SimDouble; +import com.revrobotics.CANSparkMax; -public class MockSparkMax { - // Assign the CAN port to a PWM port so it works with the simulator. Not a fan - // of this solution though - // CAN ports should be separate from PWM ports - private final int port; - private final SimDevice motor; - private final SimDouble speed; - private RelativeEncoder encoder; - private SparkAbsoluteEncoder absoluteEncoder; - private SparkPIDController pidController; - private boolean isInverted; - // Since we need to keep a record of all the motor's followers - private static ConcurrentHashMap> followMap = new ConcurrentHashMap<>(); +public class MockSparkMax extends MockSparkBase { public MockSparkMax(int port, MotorType type) { - this.port = port; - motor = SimDevice.create("SparkMax", port); - speed = motor.createDouble("Motor Output", Direction.kOutput, 0); - encoder = Mocks.createMock(RelativeEncoder.class, new MockedSparkEncoder(port), new REVLibErrorAnswer()); - absoluteEncoder = Mocks.createMock(SparkAbsoluteEncoder.class, new MockedSparkAbsoluteEncoder(port), new REVLibErrorAnswer()); - pidController = Mocks.createMock(SparkPIDController.class, new MockedSparkMaxPIDController(), new REVLibErrorAnswer()); - isInverted = false; + super(port, type, "SparkMax"); } public static CANSparkMax createMockSparkMax(int portPWM, MotorType type) { return Mocks.createMock(CANSparkMax.class, new MockSparkMax(portPWM, type), new DummySparkMaxAnswer()); } - - public void set(double speed) { - speed = (isInverted ? -1.0 : 1.0) * speed; - this.speed.set(speed); - if (followMap.containsKey(getDeviceId())) { - for (SimDouble motorOutput : followMap.get(getDeviceId())) motorOutput.set(speed); - } - } - - public REVLibError follow(CANSparkMax leader) { - return follow(leader, false); - } - - public REVLibError follow(CANSparkMax leader, boolean invert) { - return follow(ExternalFollower.kFollowerSpark, leader.getDeviceId(), invert); - } - - public REVLibError follow(ExternalFollower leader, int deviceID) { - return follow(leader, deviceID, false); - } - - public REVLibError follow(ExternalFollower leader, int deviceID, boolean invert) { - if (!followMap.containsKey(deviceID)) { - followMap.put(deviceID, new CopyOnWriteArrayList()); - } - followMap.get(deviceID).add(speed); - return REVLibError.kOk; - } - - public double get() { - return speed.get(); - } - - public int getDeviceId() { - return port; - } - - public RelativeEncoder getEncoder() { - return encoder; - } - - public SparkAbsoluteEncoder getAbsoluteEncoder() { - return absoluteEncoder; - } - - public SparkAbsoluteEncoder getAbsoluteEncoder(SparkMaxAbsoluteEncoder.Type encoderType) { - return absoluteEncoder; - } - - public SparkAbsoluteEncoder getAbsoluteEncoder(SparkAbsoluteEncoder.Type encoderType) { - return absoluteEncoder; - } - - public void setInverted(boolean inverted) { - isInverted = inverted; - } - - public boolean getInverted() { - return isInverted; - } - - public REVLibError restoreFactoryDefaults() { - return REVLibError.kOk; - } - - public REVLibError setIdleMode(IdleMode mode) { - return REVLibError.kOk; - } - - public REVLibError enableVoltageCompensation(double nominalVoltage) { - return REVLibError.kOk; - } - - public REVLibError disableVoltageCompensation() { - return REVLibError.kOk; - } - - public REVLibError setSmartCurrentLimit(int limit) { - return REVLibError.kOk; - } - - public SparkPIDController getPIDController() { - return pidController; - } - - public void setVoltage(double outputVolts) { - set(outputVolts / 12); - } -} \ No newline at end of file +} From 67457c0d0880e4f484a1b5eedc094768114da9da Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Thu, 11 Apr 2024 16:26:30 -0700 Subject: [PATCH 4/9] Add sim support for Playing With Fusion's TimeOfFlight distance sensor. --- .../lib199/SensorFactory.java | 12 +- .../MockedPlayingWithFusionTimeOfFlight.java | 101 ++++++++++ .../lib199/SensorFactoryTest.java | 40 ++++ ...ckedPlayingWithFusionTimeOfFlightTest.java | 176 ++++++++++++++++++ 4 files changed, 328 insertions(+), 1 deletion(-) create mode 100644 src/main/java/org/carlmontrobotics/lib199/sim/MockedPlayingWithFusionTimeOfFlight.java create mode 100644 src/test/java/org/carlmontrobotics/lib199/SensorFactoryTest.java create mode 100644 src/test/java/org/carlmontrobotics/lib199/sim/MockedPlayingWithFusionTimeOfFlightTest.java diff --git a/src/main/java/org/carlmontrobotics/lib199/SensorFactory.java b/src/main/java/org/carlmontrobotics/lib199/SensorFactory.java index fae919de..2bcedc5d 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/MockedPlayingWithFusionTimeOfFlight.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockedPlayingWithFusionTimeOfFlight.java new file mode 100644 index 00000000..a48f944d --- /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(); + } +} \ No newline at end of file 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 00000000..184f5940 --- /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.SimDeviceTestRule; +import org.junit.ClassRule; +import org.junit.Rule; +import org.junit.Test; + +import com.playingwithfusion.TimeOfFlight; + +public class SensorFactoryTest extends ErrStreamTest { + + @ClassRule + public static SimDeviceTestRule.Class simClassRule = new SimDeviceTestRule.Class(); + @Rule + public SimDeviceTestRule.Test simTestRule = new SimDeviceTestRule.Test(); + + @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/MockedPlayingWithFusionTimeOfFlightTest.java b/src/test/java/org/carlmontrobotics/lib199/sim/MockedPlayingWithFusionTimeOfFlightTest.java new file mode 100644 index 00000000..d0ac607c --- /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.SimDeviceTestRule; +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 SimDeviceTestRule.Class simClassRule = new SimDeviceTestRule.Class(); + @Rule + public SimDeviceTestRule.Test simTestRule = new SimDeviceTestRule.Test(); + + @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); + } + +} From 70d295829e154ebc20ee18b4b2972598dae973bb Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Thu, 18 Apr 2024 16:11:37 -0700 Subject: [PATCH 5/9] Use renamed TestRules class. --- .../java/org/carlmontrobotics/lib199/SensorFactoryTest.java | 6 +++--- .../lib199/sim/MockedPlayingWithFusionTimeOfFlightTest.java | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/test/java/org/carlmontrobotics/lib199/SensorFactoryTest.java b/src/test/java/org/carlmontrobotics/lib199/SensorFactoryTest.java index 184f5940..26251986 100644 --- a/src/test/java/org/carlmontrobotics/lib199/SensorFactoryTest.java +++ b/src/test/java/org/carlmontrobotics/lib199/SensorFactoryTest.java @@ -5,7 +5,7 @@ import static org.mockito.Mockito.mockingDetails; import org.carlmontrobotics.lib199.testUtils.ErrStreamTest; -import org.carlmontrobotics.lib199.testUtils.SimDeviceTestRule; +import org.carlmontrobotics.lib199.testUtils.TestRules; import org.junit.ClassRule; import org.junit.Rule; import org.junit.Test; @@ -15,9 +15,9 @@ public class SensorFactoryTest extends ErrStreamTest { @ClassRule - public static SimDeviceTestRule.Class simClassRule = new SimDeviceTestRule.Class(); + public static TestRules.InitializeHAL classRule = new TestRules.InitializeHAL(); @Rule - public SimDeviceTestRule.Test simTestRule = new SimDeviceTestRule.Test(); + public TestRules.ResetSimDeviceSimData testRule = new TestRules.ResetSimDeviceSimData(); @Test // AutoClosable.close() throws Exception diff --git a/src/test/java/org/carlmontrobotics/lib199/sim/MockedPlayingWithFusionTimeOfFlightTest.java b/src/test/java/org/carlmontrobotics/lib199/sim/MockedPlayingWithFusionTimeOfFlightTest.java index d0ac607c..9597e6b1 100644 --- a/src/test/java/org/carlmontrobotics/lib199/sim/MockedPlayingWithFusionTimeOfFlightTest.java +++ b/src/test/java/org/carlmontrobotics/lib199/sim/MockedPlayingWithFusionTimeOfFlightTest.java @@ -15,7 +15,7 @@ import com.playingwithfusion.TimeOfFlight.Status; import com.playingwithfusion.TimeOfFlight.RangingMode; -import org.carlmontrobotics.lib199.testUtils.SimDeviceTestRule; +import org.carlmontrobotics.lib199.testUtils.TestRules; import org.junit.ClassRule; import org.junit.Rule; import org.junit.Test; @@ -28,9 +28,9 @@ public class MockedPlayingWithFusionTimeOfFlightTest { @ClassRule - public static SimDeviceTestRule.Class simClassRule = new SimDeviceTestRule.Class(); + public static TestRules.InitializeHAL classRule = new TestRules.InitializeHAL(); @Rule - public SimDeviceTestRule.Test simTestRule = new SimDeviceTestRule.Test(); + public TestRules.ResetSimDeviceSimData testRule = new TestRules.ResetSimDeviceSimData(); @Test public void testDeviceCreation() { From 5e38df18cbb0e85c1cd088df3b92c46f5537bf05 Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Thu, 18 Apr 2024 16:27:02 -0700 Subject: [PATCH 6/9] Start adding plumbing to enable sim of SparkPIDController. --- .../sim/MockedSparkMaxPIDController.java | 49 ++++++++++++++++++- .../sim/MockedSparkMaxPIDControllerTest.java | 42 +++++++++++++++- 2 files changed, 87 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDController.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDController.java index a773758a..a0ae6c53 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDController.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDController.java @@ -1,14 +1,59 @@ package org.carlmontrobotics.lib199.sim; -import com.revrobotics.REVLibError; +import java.util.Arrays; +import com.revrobotics.CANSparkBase; +import com.revrobotics.REVLibError; +import com.revrobotics.SparkPIDController; +import com.revrobotics.CANSparkBase.ControlType; +import com.revrobotics.SparkPIDController.ArbFFUnits; + +import edu.wpi.first.hal.SimDevice; +import edu.wpi.first.hal.SimDouble; +import edu.wpi.first.hal.SimEnum; +import edu.wpi.first.hal.SimDevice.Direction; import edu.wpi.first.math.controller.PIDController; public class MockedSparkMaxPIDController { private PIDController pidController; + private int portNumber; + SimDevice pidControllerSim; + SimDouble velocitySim; + SimEnum controlTypeSim; + SimDouble arbFFSim; + SimEnum arbFFUnitsSim; + + public MockedSparkMaxPIDController(int portNumber) { + this.portNumber = portNumber; + pidController = new PIDController(0.0, 0.0, 0.0); + pidControllerSim = SimDevice.create("SparkPIDController", portNumber); + + velocitySim = pidControllerSim.createDouble("velocity", Direction.kBidir, 0); + + String[] controlTypeNames = Arrays.stream(ControlType.values()).map(ControlType::name).toArray(String[]::new); + controlTypeSim = pidControllerSim.createEnum("controlType", Direction.kBidir, controlTypeNames, ControlType.kDutyCycle.ordinal()); + + arbFFSim = pidControllerSim.createDouble("arbFF", Direction.kBidir, 0); + String[] arbFFUnitNames = Arrays.stream(ArbFFUnits.values()).map(ArbFFUnits::name).toArray(String[]::new); + arbFFUnitsSim = pidControllerSim.createEnum("arbFFUnits", Direction.kBidir, arbFFUnitNames, ArbFFUnits.kVoltage.ordinal()); + } + + @Deprecated public MockedSparkMaxPIDController() { - pidController = new PIDController(0.0, 0.0, 0.0); + this(0); + } + + public REVLibError setReference​(double value, CANSparkBase.ControlType ctrl, int pidSlot, double arbFeedforward, SparkPIDController.ArbFFUnits arbFFUnits) { + controlTypeSim.set(ctrl.ordinal()); + arbFFSim.set(arbFeedforward); + arbFFUnitsSim.set(arbFFUnits.ordinal()); + switch (ctrl) { + case kVelocity: + velocitySim.set(value); + break; + } + return REVLibError.kOk; } public REVLibError setP(double gain) { diff --git a/src/test/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDControllerTest.java b/src/test/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDControllerTest.java index 7d41b683..858bd20b 100644 --- a/src/test/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDControllerTest.java +++ b/src/test/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDControllerTest.java @@ -1,26 +1,64 @@ package org.carlmontrobotics.lib199.sim; import static org.junit.Assert.assertEquals; +import static org.hamcrest.MatcherAssert.*; +import static org.hamcrest.CoreMatchers.*; import java.util.function.BiFunction; import java.util.function.Function; import java.util.function.Supplier; +import com.revrobotics.CANPIDController; import com.revrobotics.REVLibError; +import com.revrobotics.SparkMaxPIDController; import com.revrobotics.SparkPIDController; +import com.revrobotics.CANSparkBase.ControlType; +import com.revrobotics.SparkPIDController.ArbFFUnits; + +import edu.wpi.first.hal.SimDouble; +import edu.wpi.first.hal.SimEnum; +import edu.wpi.first.wpilibj.simulation.SimDeviceSim; import org.carlmontrobotics.lib199.Mocks; import org.carlmontrobotics.lib199.REVLibErrorAnswer; +import org.carlmontrobotics.lib199.testUtils.TestRules; +import org.junit.ClassRule; +import org.junit.Rule; import org.junit.Test; public class MockedSparkMaxPIDControllerTest { - + + @ClassRule + public static TestRules.InitializeHAL classRule = new TestRules.InitializeHAL(); + @Rule + public TestRules.ResetSimDeviceSimData testRule = new TestRules.ResetSimDeviceSimData(); + + @Test public void testResponses() { - SparkPIDController mock = Mocks.createMock(SparkPIDController.class, new MockedSparkMaxPIDController(), new REVLibErrorAnswer()); + SparkPIDController mock = Mocks.createMock(SparkPIDController.class, new MockedSparkMaxPIDController(1), + new REVLibErrorAnswer(), CANPIDController.class, SparkMaxPIDController.class); assertSlotValueUpdate(mock::setP, mock::setP, mock::getP, mock::getP); assertSlotValueUpdate(mock::setI, mock::setI, mock::getI, mock::getI); assertSlotValueUpdate(mock::setD, mock::setD, mock::getD, mock::getD); + + + SimDeviceSim pidControllerSim = new SimDeviceSim("SparkPIDController", 1); + SimDouble velocitySim = pidControllerSim.getDouble("velocity"); + SimEnum controlTypeSim = pidControllerSim.getEnum("controlType"); + SimDouble arbFFSim = pidControllerSim.getDouble("arbFF"); + SimEnum arbFFUnitsSim = pidControllerSim.getEnum("arbFFUnits"); + + assertThat(velocitySim.get(), is(0.0)); + assertThat(controlTypeSim.get(), is(ControlType.kDutyCycle.ordinal())); + assertThat(arbFFSim.get(), is(0.0)); + assertThat(arbFFUnitsSim.get(), is(ArbFFUnits.kVoltage.ordinal())); + + assertEquals(REVLibError.kOk, mock.setReference(0.5, ControlType.kVelocity, 1, 0.25, ArbFFUnits.kPercentOut)); + assertThat(velocitySim.get(), is(0.5)); + assertThat(controlTypeSim.get(), is(ControlType.kVelocity.ordinal())); + assertThat(arbFFSim.get(), is(0.25)); + assertThat(arbFFUnitsSim.get(), is(ArbFFUnits.kPercentOut.ordinal())); } private void assertSlotValueUpdate(Function setFunc, BiFunction slotSetFunc, Supplier getFunc, Function slotGetFunc) { From 95a06145e12ca6f067e681d792d72f9edea78ca4 Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Fri, 19 Apr 2024 13:32:51 -0700 Subject: [PATCH 7/9] Allow setReference() calls to be tracked atomically. --- .../sim/MockedSparkMaxPIDController.java | 37 ++++++++---- .../sim/MockedSparkMaxPIDControllerTest.java | 57 ++++++++++++++++++- 2 files changed, 79 insertions(+), 15 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDController.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDController.java index a0ae6c53..5fcc4f5f 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDController.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDController.java @@ -8,9 +8,12 @@ import com.revrobotics.CANSparkBase.ControlType; import com.revrobotics.SparkPIDController.ArbFFUnits; +import edu.wpi.first.hal.SimBoolean; import edu.wpi.first.hal.SimDevice; import edu.wpi.first.hal.SimDouble; import edu.wpi.first.hal.SimEnum; +import edu.wpi.first.hal.SimInt; +import edu.wpi.first.hal.SimLong; import edu.wpi.first.hal.SimDevice.Direction; import edu.wpi.first.math.controller.PIDController; @@ -18,8 +21,11 @@ public class MockedSparkMaxPIDController { private PIDController pidController; private int portNumber; SimDevice pidControllerSim; - SimDouble velocitySim; + SimBoolean isUpdatingReferenceSim; + SimLong numUpdatesSim; + SimDouble referenceSim; SimEnum controlTypeSim; + SimInt slotSim; SimDouble arbFFSim; SimEnum arbFFUnitsSim; @@ -28,15 +34,19 @@ public MockedSparkMaxPIDController(int portNumber) { pidController = new PIDController(0.0, 0.0, 0.0); pidControllerSim = SimDevice.create("SparkPIDController", portNumber); - velocitySim = pidControllerSim.createDouble("velocity", Direction.kBidir, 0); + isUpdatingReferenceSim = pidControllerSim.createBoolean("isUpdating", Direction.kOutput, false); + numUpdatesSim = pidControllerSim.createLong("numUpdates", Direction.kOutput, 0); + + referenceSim = pidControllerSim.createDouble("reference", Direction.kOutput, 0); String[] controlTypeNames = Arrays.stream(ControlType.values()).map(ControlType::name).toArray(String[]::new); - controlTypeSim = pidControllerSim.createEnum("controlType", Direction.kBidir, controlTypeNames, ControlType.kDutyCycle.ordinal()); + controlTypeSim = pidControllerSim.createEnum("controlType", Direction.kOutput, controlTypeNames, ControlType.kDutyCycle.ordinal()); - arbFFSim = pidControllerSim.createDouble("arbFF", Direction.kBidir, 0); + slotSim = pidControllerSim.createInt("slot", Direction.kOutput, 0); + arbFFSim = pidControllerSim.createDouble("arbFF", Direction.kOutput, 0); String[] arbFFUnitNames = Arrays.stream(ArbFFUnits.values()).map(ArbFFUnits::name).toArray(String[]::new); - arbFFUnitsSim = pidControllerSim.createEnum("arbFFUnits", Direction.kBidir, arbFFUnitNames, ArbFFUnits.kVoltage.ordinal()); + arbFFUnitsSim = pidControllerSim.createEnum("arbFFUnits", Direction.kOutput, arbFFUnitNames, ArbFFUnits.kVoltage.ordinal()); } @Deprecated @@ -45,13 +55,16 @@ public MockedSparkMaxPIDController() { } public REVLibError setReference​(double value, CANSparkBase.ControlType ctrl, int pidSlot, double arbFeedforward, SparkPIDController.ArbFFUnits arbFFUnits) { - controlTypeSim.set(ctrl.ordinal()); - arbFFSim.set(arbFeedforward); - arbFFUnitsSim.set(arbFFUnits.ordinal()); - switch (ctrl) { - case kVelocity: - velocitySim.set(value); - break; + isUpdatingReferenceSim.set(true); + try { + referenceSim.set(value); + controlTypeSim.set(ctrl.ordinal()); + slotSim.set(pidSlot); + arbFFSim.set(arbFeedforward); + arbFFUnitsSim.set(arbFFUnits.ordinal()); + } finally { + numUpdatesSim.set(numUpdatesSim.get()+1); + isUpdatingReferenceSim.set(false); } return REVLibError.kOk; } diff --git a/src/test/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDControllerTest.java b/src/test/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDControllerTest.java index 858bd20b..5ab41a09 100644 --- a/src/test/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDControllerTest.java +++ b/src/test/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDControllerTest.java @@ -15,8 +15,13 @@ import com.revrobotics.CANSparkBase.ControlType; import com.revrobotics.SparkPIDController.ArbFFUnits; +import edu.wpi.first.hal.HALValue; +import edu.wpi.first.hal.SimBoolean; import edu.wpi.first.hal.SimDouble; import edu.wpi.first.hal.SimEnum; +import edu.wpi.first.hal.SimInt; +import edu.wpi.first.hal.SimLong; +import edu.wpi.first.hal.simulation.SimValueCallback; import edu.wpi.first.wpilibj.simulation.SimDeviceSim; import org.carlmontrobotics.lib199.Mocks; @@ -33,6 +38,7 @@ public class MockedSparkMaxPIDControllerTest { @Rule public TestRules.ResetSimDeviceSimData testRule = new TestRules.ResetSimDeviceSimData(); + private int numChangesToIsUpdating = 0; @Test public void testResponses() { @@ -44,21 +50,66 @@ public void testResponses() { SimDeviceSim pidControllerSim = new SimDeviceSim("SparkPIDController", 1); - SimDouble velocitySim = pidControllerSim.getDouble("velocity"); + SimBoolean isUpdatingSim = pidControllerSim.getBoolean("isUpdating"); + SimLong numUpdatesSim = pidControllerSim.getLong("numUpdates"); + SimDouble referenceSim = pidControllerSim.getDouble("reference"); SimEnum controlTypeSim = pidControllerSim.getEnum("controlType"); + SimInt slotSim = pidControllerSim.getInt("slot"); SimDouble arbFFSim = pidControllerSim.getDouble("arbFF"); SimEnum arbFFUnitsSim = pidControllerSim.getEnum("arbFFUnits"); - assertThat(velocitySim.get(), is(0.0)); + pidControllerSim.registerValueChangedCallback(isUpdatingSim, new SimValueCallback() { + @Override + public void callback(String name, int handle, int direction, HALValue value) { + numChangesToIsUpdating++; + } + }, false); + + // Defaults + assertThat(numChangesToIsUpdating, is(0)); + assertThat(isUpdatingSim.get(), is(false)); + assertThat(numUpdatesSim.get(), is(0L)); + assertThat(referenceSim.get(), is(0.0)); assertThat(controlTypeSim.get(), is(ControlType.kDutyCycle.ordinal())); + assertThat(slotSim.get(), is(0)); assertThat(arbFFSim.get(), is(0.0)); assertThat(arbFFUnitsSim.get(), is(ArbFFUnits.kVoltage.ordinal())); + // Velocity control assertEquals(REVLibError.kOk, mock.setReference(0.5, ControlType.kVelocity, 1, 0.25, ArbFFUnits.kPercentOut)); - assertThat(velocitySim.get(), is(0.5)); + assertThat(numChangesToIsUpdating, is(2)); + assertThat(isUpdatingSim.get(), is(false)); + assertThat(numUpdatesSim.get(), is(1L)); + assertThat(referenceSim.get(), is(0.5)); assertThat(controlTypeSim.get(), is(ControlType.kVelocity.ordinal())); + assertThat(slotSim.get(), is(1)); assertThat(arbFFSim.get(), is(0.25)); assertThat(arbFFUnitsSim.get(), is(ArbFFUnits.kPercentOut.ordinal())); + assertThat(numUpdatesSim.get(), is(1L)); + + // Duty-cycle control (the default) + assertEquals(REVLibError.kOk, mock.setReference(0.6, ControlType.kDutyCycle, 1, 0.35, ArbFFUnits.kVoltage)); + assertThat(numChangesToIsUpdating, is(4)); + assertThat(isUpdatingSim.get(), is(false)); + assertThat(numUpdatesSim.get(), is(2L)); + assertThat(referenceSim.get(), is(0.6)); + assertThat(controlTypeSim.get(), is(ControlType.kDutyCycle.ordinal())); + assertThat(slotSim.get(), is(1)); + assertThat(arbFFSim.get(), is(0.35)); + assertThat(arbFFUnitsSim.get(), is(ArbFFUnits.kVoltage.ordinal())); + assertThat(numUpdatesSim.get(), is(2L)); + + // Position control with slot 2 + assertEquals(REVLibError.kOk, mock.setReference(0.5, ControlType.kPosition, 2, 0.25, ArbFFUnits.kPercentOut)); + assertThat(numChangesToIsUpdating, is(6)); + assertThat(isUpdatingSim.get(), is(false)); + assertThat(numUpdatesSim.get(), is(3L)); + assertThat(referenceSim.get(), is(0.5)); + assertThat(controlTypeSim.get(), is(ControlType.kPosition.ordinal())); + assertThat(slotSim.get(), is(2)); + assertThat(arbFFSim.get(), is(0.25)); + assertThat(arbFFUnitsSim.get(), is(ArbFFUnits.kPercentOut.ordinal())); + assertThat(numUpdatesSim.get(), is(3L)); } private void assertSlotValueUpdate(Function setFunc, BiFunction slotSetFunc, Supplier getFunc, Function slotGetFunc) { From 8abb677fc7ff03a019c6d93628098b24c58d6e92 Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Sat, 20 Apr 2024 14:03:53 -0700 Subject: [PATCH 8/9] Store sim values for p, i, and d gains for each slot. --- .../sim/MockedSparkMaxPIDController.java | 63 +++++++++++-------- .../sim/MockedSparkMaxPIDControllerTest.java | 53 +++++++--------- 2 files changed, 59 insertions(+), 57 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDController.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDController.java index 5fcc4f5f..1ae05b5b 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDController.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDController.java @@ -15,24 +15,35 @@ import edu.wpi.first.hal.SimInt; import edu.wpi.first.hal.SimLong; import edu.wpi.first.hal.SimDevice.Direction; -import edu.wpi.first.math.controller.PIDController; public class MockedSparkMaxPIDController { - private PIDController pidController; + private static class ControlParameters { + SimDouble pSim, iSim, dSim; + + ControlParameters(SimDevice device, int slot) { + pSim = device.createDouble(String.format("p[%d]", slot), Direction.kOutput, 0); + iSim = device.createDouble(String.format("i[%d]", slot), Direction.kOutput, 0); + dSim = device.createDouble(String.format("d[%d]", slot), Direction.kOutput, 0); + } + } + private ControlParameters[] controlParams = new ControlParameters[4]; + private int portNumber; - SimDevice pidControllerSim; - SimBoolean isUpdatingReferenceSim; - SimLong numUpdatesSim; - SimDouble referenceSim; - SimEnum controlTypeSim; - SimInt slotSim; - SimDouble arbFFSim; - SimEnum arbFFUnitsSim; + private SimDevice pidControllerSim; + private SimBoolean isUpdatingReferenceSim; + private SimLong numUpdatesSim; + private SimDouble referenceSim; + private SimEnum controlTypeSim; + private SimInt slotSim; + private SimDouble arbFFSim; + private SimEnum arbFFUnitsSim; public MockedSparkMaxPIDController(int portNumber) { this.portNumber = portNumber; - pidController = new PIDController(0.0, 0.0, 0.0); pidControllerSim = SimDevice.create("SparkPIDController", portNumber); + for (int i : new int[] {0, 1, 2, 3}) { + controlParams[i] = new ControlParameters(pidControllerSim, i); + } isUpdatingReferenceSim = pidControllerSim.createBoolean("isUpdating", Direction.kOutput, false); numUpdatesSim = pidControllerSim.createLong("numUpdates", Direction.kOutput, 0); @@ -70,53 +81,53 @@ public MockedSparkMaxPIDController() { } public REVLibError setP(double gain) { - pidController.setP(gain); - return REVLibError.kOk; + return setP(gain, 0); } public REVLibError setP(double gain, int slotID) { - return setP(gain); + controlParams[slotID].pSim.set(gain); + return REVLibError.kOk; } public double getP() { - return pidController.getP(); + return getP(0); } public double getP(int slotID) { - return getP(); + return controlParams[slotID].pSim.get(); } public REVLibError setI(double gain) { - pidController.setI(gain); - return REVLibError.kOk; + return setI(gain, 0); } public REVLibError setI(double gain, int slotID) { - return setI(gain); + controlParams[slotID].iSim.set(gain); + return REVLibError.kOk; } public double getI() { - return pidController.getI(); + return getI(0); } public double getI(int slotID) { - return getI(); + return controlParams[slotID].iSim.get(); } public REVLibError setD(double gain) { - pidController.setD(gain); - return REVLibError.kOk; + return setD(gain, 0); } public REVLibError setD(double gain, int slotID) { - return setD(gain); + controlParams[slotID].dSim.set(gain); + return REVLibError.kOk; } public double getD() { - return pidController.getD(); + return getD(0); } public double getD(int slotID) { - return getD(); + return controlParams[slotID].dSim.get(); } } diff --git a/src/test/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDControllerTest.java b/src/test/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDControllerTest.java index 5ab41a09..90fe2c5d 100644 --- a/src/test/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDControllerTest.java +++ b/src/test/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDControllerTest.java @@ -44,11 +44,6 @@ public class MockedSparkMaxPIDControllerTest { public void testResponses() { SparkPIDController mock = Mocks.createMock(SparkPIDController.class, new MockedSparkMaxPIDController(1), new REVLibErrorAnswer(), CANPIDController.class, SparkMaxPIDController.class); - assertSlotValueUpdate(mock::setP, mock::setP, mock::getP, mock::getP); - assertSlotValueUpdate(mock::setI, mock::setI, mock::getI, mock::getI); - assertSlotValueUpdate(mock::setD, mock::setD, mock::getD, mock::getD); - - SimDeviceSim pidControllerSim = new SimDeviceSim("SparkPIDController", 1); SimBoolean isUpdatingSim = pidControllerSim.getBoolean("isUpdating"); SimLong numUpdatesSim = pidControllerSim.getLong("numUpdates"); @@ -57,7 +52,6 @@ public void testResponses() { SimInt slotSim = pidControllerSim.getInt("slot"); SimDouble arbFFSim = pidControllerSim.getDouble("arbFF"); SimEnum arbFFUnitsSim = pidControllerSim.getEnum("arbFFUnits"); - pidControllerSim.registerValueChangedCallback(isUpdatingSim, new SimValueCallback() { @Override public void callback(String name, int handle, int direction, HALValue value) { @@ -65,6 +59,13 @@ public void callback(String name, int handle, int direction, HALValue value) { } }, false); + assertSlotValueUpdate(pidControllerSim, "p", mock::setP, mock::setP, mock::getP, mock::getP, + (slot) -> pidControllerSim.getDouble(String.format("p[%d]", slot)).get()); + assertSlotValueUpdate(pidControllerSim, "i", mock::setI, mock::setI, mock::getI, mock::getI, + (slot) -> pidControllerSim.getDouble(String.format("i[%d]", slot)).get()); + assertSlotValueUpdate(pidControllerSim, "d", mock::setD, mock::setD, mock::getD, mock::getD, + (slot) -> pidControllerSim.getDouble(String.format("d[%d]", slot)).get()); + // Defaults assertThat(numChangesToIsUpdating, is(0)); assertThat(isUpdatingSim.get(), is(false)); @@ -112,31 +113,21 @@ public void callback(String name, int handle, int direction, HALValue value) { assertThat(numUpdatesSim.get(), is(3L)); } - private void assertSlotValueUpdate(Function setFunc, BiFunction slotSetFunc, Supplier getFunc, Function slotGetFunc) { - assertSlotValueUpdate(setFunc, getFunc, slotGetFunc); - assertSlotValueUpdate(v -> slotSetFunc.apply(v, 0), getFunc, slotGetFunc); - assertSlotValueUpdate(v -> slotSetFunc.apply(v, 1), getFunc, slotGetFunc); - assertSlotValueUpdate(v -> slotSetFunc.apply(v, 2), getFunc, slotGetFunc); - } - - private void assertSlotValueUpdate(Function setFunc, Supplier getFunc, Function slotGetFunc) { - assertEquals(REVLibError.kOk, setFunc.apply(0.0)); - assertSlotValueGet(0, getFunc, slotGetFunc); + private void assertSlotValueUpdate(SimDeviceSim pidControllerSim, String paramName, Function setFunc, BiFunction slotSetFunc, Supplier getFunc, Function slotGetFunc, Function simValueGetFunc) { assertEquals(REVLibError.kOk, setFunc.apply(1.0)); - assertSlotValueGet(1, getFunc, slotGetFunc); - assertEquals(REVLibError.kOk, setFunc.apply(0.5)); - assertSlotValueGet(0.5, getFunc, slotGetFunc); - assertEquals(REVLibError.kOk, setFunc.apply(-0.5)); - assertSlotValueGet(-0.5, getFunc, slotGetFunc); - assertEquals(REVLibError.kOk, setFunc.apply(-1.0)); - assertSlotValueGet(-1, getFunc, slotGetFunc); - } - - private void assertSlotValueGet(double expected, Supplier getFunc, Function slotGetFunc) { - assertEquals(expected, getFunc.get(), 0.01); - assertEquals(expected, slotGetFunc.apply(0), 0.01); - assertEquals(expected, slotGetFunc.apply(1), 0.01); - assertEquals(expected, slotGetFunc.apply(2), 0.01); + assertThat(getFunc.get(), is(1.0)); + assertEquals(REVLibError.kOk, slotSetFunc.apply(2.0, 0)); + assertEquals(REVLibError.kOk, slotSetFunc.apply(3.0, 1)); + assertEquals(REVLibError.kOk, slotSetFunc.apply(4.0, 2)); + assertEquals(REVLibError.kOk, slotSetFunc.apply(5.0, 3)); + assertThat(getFunc.get(), is(2.0)); + assertThat(slotGetFunc.apply(0), is(2.0)); + assertThat(slotGetFunc.apply(1), is(3.0)); + assertThat(slotGetFunc.apply(2), is(4.0)); + assertThat(slotGetFunc.apply(3), is(5.0)); + assertThat(simValueGetFunc.apply(0), is(2.0)); + assertThat(simValueGetFunc.apply(1), is(3.0)); + assertThat(simValueGetFunc.apply(2), is(4.0)); + assertThat(simValueGetFunc.apply(3), is(5.0)); } - } From 01ccacbfa2773d419b516f8fa5112bdc83b868bd Mon Sep 17 00:00:00 2001 From: CoolSpy3 Date: Fri, 17 May 2024 17:30:18 -0700 Subject: [PATCH 9/9] formatting --- .../lib199/MotorControllerFactory.java | 6 +++--- .../org/carlmontrobotics/lib199/MotorErrors.java | 12 ++++++------ .../lib199/sim/MockSparkBase.java | 2 +- .../sim/MockedPlayingWithFusionTimeOfFlight.java | 4 ++-- .../lib199/sim/MockSparkMaxTest.java | 15 ++------------- .../MockedPlayingWithFusionTimeOfFlightTest.java | 6 +++--- 6 files changed, 17 insertions(+), 28 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/MotorControllerFactory.java b/src/main/java/org/carlmontrobotics/lib199/MotorControllerFactory.java index d6d6b0fb..0cd62ee7 100644 --- a/src/main/java/org/carlmontrobotics/lib199/MotorControllerFactory.java +++ b/src/main/java/org/carlmontrobotics/lib199/MotorControllerFactory.java @@ -124,7 +124,7 @@ public static CANSparkMax createSparkMax(int id, MotorConfig config) { } configureSpark(spark, config); - + return spark; } @@ -137,7 +137,7 @@ public static CANSparkFlex createSparkFlex(int id, MotorConfig config) { } configureSpark(spark, config); - + return spark; } @@ -203,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 7eed4bb6..6cacbe18 100644 --- a/src/main/java/org/carlmontrobotics/lib199/MotorErrors.java +++ b/src/main/java/org/carlmontrobotics/lib199/MotorErrors.java @@ -79,7 +79,7 @@ public static void checkSparkErrors(CANSparkBase spark) { public static void checkSparkMaxErrors(CANSparkMax spark) { checkSparkErrors((CANSparkBase)spark); } - + private static String formatFaults(CANSparkBase spark) { String out = ""; for(FaultID fault: FaultID.values()) { @@ -106,7 +106,7 @@ public static void printSparkMaxErrorMessages() { } public static void printSparkErrorMessages() { - flags.keySet().forEach((spark) -> checkSparkErrors(spark)); + flags.keySet().forEach(MotorErrors::checkSparkErrors); } public static CANSparkMax createDummySparkMax() { @@ -117,7 +117,7 @@ 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); @@ -126,9 +126,9 @@ public static boolean isSparkMaxOverheated(CANSparkMax spark){ @Deprecated public static void reportSparkMaxTemp(CANSparkMax spark, int temperatureLimit) { - reportSparkTemp((CANSparkBase)spark, temperatureLimit); + reportSparkTemp((CANSparkBase) spark, temperatureLimit); } - + public static void reportSparkTemp(CANSparkBase spark, int temperatureLimit) { int id = spark.getDeviceId(); temperatureSparks.put(id, spark); @@ -188,4 +188,4 @@ private TemperatureLimit(int limit) { } } -} \ No newline at end of file +} diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkBase.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkBase.java index 45453fe9..cc63577b 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkBase.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkBase.java @@ -40,7 +40,7 @@ public class MockSparkBase extends MockedMotorBase { /** * 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. diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockedPlayingWithFusionTimeOfFlight.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockedPlayingWithFusionTimeOfFlight.java index a48f944d..58e9597d 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockedPlayingWithFusionTimeOfFlight.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockedPlayingWithFusionTimeOfFlight.java @@ -69,7 +69,7 @@ public RangingMode getRangingMode() { public double getSampleTime() { return sampleTime.get(); } - + public Status getStatus() { return Status.values()[status.get()]; } @@ -98,4 +98,4 @@ public void setRangingMode(RangingMode newMode, double newSampleTime) { public void close() { device.close(); } -} \ No newline at end of file +} diff --git a/src/test/java/org/carlmontrobotics/lib199/sim/MockSparkMaxTest.java b/src/test/java/org/carlmontrobotics/lib199/sim/MockSparkMaxTest.java index 0745f075..e4e0d07d 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 index 9597e6b1..82e84275 100644 --- a/src/test/java/org/carlmontrobotics/lib199/sim/MockedPlayingWithFusionTimeOfFlightTest.java +++ b/src/test/java/org/carlmontrobotics/lib199/sim/MockedPlayingWithFusionTimeOfFlightTest.java @@ -28,9 +28,9 @@ public class MockedPlayingWithFusionTimeOfFlightTest { @ClassRule - public static TestRules.InitializeHAL classRule = new TestRules.InitializeHAL(); + public static TestRules.InitializeHAL classRule = new TestRules.InitializeHAL(); @Rule - public TestRules.ResetSimDeviceSimData testRule = new TestRules.ResetSimDeviceSimData(); + public TestRules.ResetSimDeviceSimData testRule = new TestRules.ResetSimDeviceSimData(); @Test public void testDeviceCreation() { @@ -172,5 +172,5 @@ private void withDevice(int id, EncoderTest func) { private interface EncoderTest { public void test(TimeOfFlight encoder, SimDeviceSim sim); } - + }