Fix Sporadic Temperature Reporting

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

@@ -0,0 +1,15 @@
+package org.carlmontrobotics.lib199;
+
+public class MotorConfig {
+
+ public static final MotorConfig NEO = new MotorConfig(70, 40);
+ public static final MotorConfig NEO_550 = new MotorConfig(40, 20);
+
+ public final int temperatureLimitCelsius, currentLimitAmps;
+
+ public MotorConfig(int temperatureLimitCelsius, int currentLimitAmps) {
+ this.temperatureLimitCelsius = temperatureLimitCelsius;
+ this.currentLimitAmps = currentLimitAmps;
+ }
+
+}

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

@@ -17,7 +17,6 @@
 import com.revrobotics.CANSparkMaxLowLevel;
 import com.revrobotics.SparkMaxPIDController;
 
-import org.carlmontrobotics.MotorConfig;
 import org.carlmontrobotics.lib199.sim.MockSparkMax;
 import org.carlmontrobotics.lib199.sim.MockTalonSRX;
 import org.carlmontrobotics.lib199.sim.MockVictorSPX;
@@ -125,13 +124,13 @@ public static CANSparkMax createSparkMax(int id, MotorConfig config) {
  spark = MockSparkMax.createMockSparkMax(id, CANSparkMaxLowLevel.MotorType.kBrushless);
  }
 
- MotorErrors.reportSparkMaxTemp(spark, config.temperatureLimit);
+ MotorErrors.reportSparkMaxTemp(spark, config.temperatureLimitCelsius);
 
  MotorErrors.reportError(spark.restoreFactoryDefaults());
  MotorErrors.reportError(spark.follow(ExternalFollower.kFollowerDisabled, 0));
  MotorErrors.reportError(spark.setIdleMode(IdleMode.kBrake));
  MotorErrors.reportError(spark.enableVoltageCompensation(12));
- MotorErrors.reportError(spark.setSmartCurrentLimit(config.currentLimit));
+ MotorErrors.reportError(spark.setSmartCurrentLimit(config.currentLimitAmps));
 
  MotorErrors.checkSparkMaxErrors(spark);
 

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

@@ -2,7 +2,6 @@
 
 import java.util.Arrays;
 import java.util.concurrent.ConcurrentHashMap;
-import java.util.concurrent.CopyOnWriteArrayList;
 
 import com.ctre.phoenix.ErrorCode;
 import com.revrobotics.CANSparkMax;
@@ -15,12 +14,15 @@ public final class MotorErrors {
 
  private static final ConcurrentHashMap<Integer, CANSparkMax> temperatureSparks = new ConcurrentHashMap<>();
  private static final ConcurrentHashMap<Integer, Integer> sparkTemperatureLimits = new ConcurrentHashMap<>();
- private static final CopyOnWriteArrayList<Integer> overheatedSparks = new CopyOnWriteArrayList<>();
+ private static final ConcurrentHashMap<Integer, Integer> overheatedSparks = new ConcurrentHashMap<>();
  private static final ConcurrentHashMap<CANSparkMax, Short> flags = new ConcurrentHashMap<>();
  private static final ConcurrentHashMap<CANSparkMax, Short> stickyFlags = new ConcurrentHashMap<>();
 
+ public static final int kOverheatTripCount = 5;
+
  static {
  Lib199Subsystem.registerAsyncPeriodic(MotorErrors::doReportSparkMaxTemp);
+ Lib199Subsystem.registerAsyncPeriodic(MotorErrors::printSparkMaxErrorMessages);
  }
 
  public static void reportError(ErrorCode error) {
@@ -108,16 +110,30 @@ public static void reportSparkMaxTemp(CANSparkMax spark, int temperatureLimit) {
  int id = spark.getDeviceId();
  temperatureSparks.put(id, spark);
  sparkTemperatureLimits.put(id, temperatureLimit);
+ overheatedSparks.put(id, 0);
  }
 
  public static void doReportSparkMaxTemp() {
  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);
+
+ if(numTrips < kOverheatTripCount) {
+ if(temp > limit) {
+ overheatedSparks.put(port, ++numTrips);
+ } else {
+ overheatedSparks.put(port, 0);
+ }
+ }
+
  // Check if temperature exceeds the setpoint or if the controller has already overheated to prevent other code from resetting the current limit after the controller has cooled
- if(temp >= sparkTemperatureLimits.get(port) || overheatedSparks.contains(port)) {
- if(!overheatedSparks.contains(port)) {
- overheatedSparks.add(port);
+ if(numTrips >= kOverheatTripCount) {
+ if(numTrips < kOverheatTripCount + 1) {
+ // 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.");
  }
  spark.setSmartCurrentLimit(1);

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

@@ -191,7 +191,7 @@ private double calculateAntiGravitationalA(Float gyroPitchDeg, Float gyroRollDeg
  double g = 9.81; //meters per second squared
  // gravitationalA is estimated to work for small angles, not 100% accurate at large angles
  double antiGravitationalA = g * (modulePitchComponent * Math.sin(Math.PI * gyroPitchDeg / 180) - moduleRollComponent * Math.sin(Math.PI * gyroRollDeg / 180));
- SmartDashboard.putNumber("AntiGravitational accelration", antiGravitationalA);
+ SmartDashboard.putNumber("AntiGravitational acceleration", antiGravitationalA);
  return antiGravitationalA;
  }
 

src/test/java/org/carlmontrobotics/lib199/MotorControllerFactoryTest.java

@@ -2,7 +2,6 @@
 
 import static org.junit.Assert.assertEquals;
 
-import org.carlmontrobotics.MotorConfig;
 import org.carlmontrobotics.lib199.testUtils.ErrStreamTest;
 import org.carlmontrobotics.lib199.testUtils.SimDeviceTestRule;
 import org.junit.ClassRule;

src/test/java/org/carlmontrobotics/lib199/MotorErrorsTest.java

@@ -2,8 +2,11 @@
 
 import static org.junit.Assert.assertEquals;
 import static org.junit.Assert.assertNotEquals;
+import static org.junit.Assert.assertTrue;
 import static org.junit.Assume.assumeNoException;
 
+import java.util.concurrent.atomic.AtomicBoolean;
+
 import com.ctre.phoenix.ErrorCode;
 import com.revrobotics.REVLibError;
 import com.revrobotics.CANSparkMax;
@@ -45,7 +48,7 @@ public static interface TemperatureSparkMax {
 
  public static class Instance {
 
- private int smartCurrentLimit = 50;
+ private int smartCurrentLimit = MotorConfig.NEO.currentLimitAmps;
  private double temperature = 30;
  private final int id;
 
@@ -69,24 +72,14 @@ public REVLibError setSmartCurrentLimit(int limit) {
  smartCurrentLimit = limit;
  return REVLibError.kOk;
  }
-
+
  public int getDeviceId() {
  return id;
  }
  }
 
  }
 
- private static final Object asyncPeriodicNotifier = new Object();
-
- static {
- Lib199Subsystem.registerAsyncPeriodic(() -> {
- synchronized(asyncPeriodicNotifier) {
- asyncPeriodicNotifier.notifyAll();
- }
- });
- }
-
  @Test
  public void testOkErrors() {
  errStream.reset();
@@ -99,7 +92,7 @@ public void testOkErrors() {
  assertEquals(0, errStream.toByteArray().length);
  MotorErrors.reportErrors((REVLibError)null, null);
  assertEquals(0, errStream.toByteArray().length);
-
+
  // Ok Status
  MotorErrors.reportError(ErrorCode.OK);
  assertEquals(0, errStream.toByteArray().length);
@@ -165,53 +158,99 @@ public void testDummySparkMax() {
 
  @Test
  public void testReportSparkMaxTemp() {
+ assertTrue(MotorErrors.kOverheatTripCount > 0);
+
  doTestReportSparkMaxTemp(0);
  doTestReportSparkMaxTemp(1);
  doTestReportSparkMaxTemp(2);
  }
 
  private void doTestReportSparkMaxTemp(int id) {
  TemperatureSparkMax spark = (TemperatureSparkMax)Mocks.createMock(CANSparkMax.class, new TemperatureSparkMax.Instance(id), TemperatureSparkMax.class);
- MotorErrors.reportSparkMaxTemp((CANSparkMax)spark, 40);
- spark.setSmartCurrentLimit(50);
- spark.setTemperature(20);
- runAsyncPeriodic();
  String smartDashboardKey = "Port " + id + " Spark Max Temp";
- assertEquals(20, SmartDashboard.getNumber(smartDashboardKey, 0), 0.01);
- assertEquals(50, spark.getSmartCurrentLimit());
- spark.setTemperature(20);
- runAsyncPeriodic();
- assertEquals(20, SmartDashboard.getNumber(smartDashboardKey, 0), 0.01);
- assertEquals(50, spark.getSmartCurrentLimit());
- assertEquals(0, errStream.size());
- spark.setTemperature(40);
- runAsyncPeriodic();
- assertEquals(40, SmartDashboard.getNumber(smartDashboardKey, 0), 0.01);
- assertEquals(1, spark.getSmartCurrentLimit());
- assertNotEquals(0, errStream.size());
- errStream.reset();
- spark.setTemperature(50);
- runAsyncPeriodic();
- assertEquals(50, SmartDashboard.getNumber(smartDashboardKey, 0), 0.01);
- assertEquals(1, spark.getSmartCurrentLimit());
- spark.setTemperature(20);
- runAsyncPeriodic();
- assertEquals(20, SmartDashboard.getNumber(smartDashboardKey, 0), 0.01);
- assertEquals(1, spark.getSmartCurrentLimit());
- assertEquals(0, errStream.size());
- }
+ MotorErrors.reportSparkMaxTemp((CANSparkMax)spark, 40);
+
+ try(AutoCloseable asyncBlock = blockAsyncPeriodic()) {
+ spark.setTemperature(20);
+ spark.setSmartCurrentLimit(50);
+ MotorErrors.doReportSparkMaxTemp();
+ assertEquals(20, SmartDashboard.getNumber(smartDashboardKey, 0), 0.01);
+ assertEquals(50, spark.getSmartCurrentLimit());
+
+ spark.setTemperature(20);
+ spark.setSmartCurrentLimit(50);
+ MotorErrors.doReportSparkMaxTemp();
+ assertEquals(20, SmartDashboard.getNumber(smartDashboardKey, 0), 0.01);
+ assertEquals(50, spark.getSmartCurrentLimit());
 
- // Ensures an update to the asynchronous periodic thread is run
- private void runAsyncPeriodic() {
- try {
- synchronized(asyncPeriodicNotifier) {
- // Run twice because we don't know in what order we're called, so make sure all periodic methods are run twice
- asyncPeriodicNotifier.wait();
- asyncPeriodicNotifier.wait();
+ if(MotorErrors.kOverheatTripCount > 1) {
+ spark.setTemperature(51);
+ spark.setSmartCurrentLimit(50);
+ MotorErrors.doReportSparkMaxTemp();
+ assertEquals(51, SmartDashboard.getNumber(smartDashboardKey, 0), 0.01);
+ assertEquals(50, spark.getSmartCurrentLimit());
+
+ spark.setTemperature(20);
+ spark.setSmartCurrentLimit(50);
+ MotorErrors.doReportSparkMaxTemp();
+ assertEquals(20, SmartDashboard.getNumber(smartDashboardKey, 0), 0.01);
+ assertEquals(50, spark.getSmartCurrentLimit());
+ }
+
+ assertEquals(0, errStream.size());
+
+ for(int i = 0; i < MotorErrors.kOverheatTripCount; i++) {
+ assertEquals(50, spark.getSmartCurrentLimit());
+ assertEquals(0, errStream.size());
+
+ spark.setTemperature(51);
+ spark.setSmartCurrentLimit(50);
+ MotorErrors.doReportSparkMaxTemp();
+ assertEquals(51, SmartDashboard.getNumber(smartDashboardKey, 0), 0.01);
  }
- } catch(InterruptedException e) {
+
+ assertNotEquals(0, errStream.size());
+ errStream.reset();
+
+ spark.setTemperature(51);
+ spark.setSmartCurrentLimit(50);
+ MotorErrors.doReportSparkMaxTemp();
+ assertEquals(51, SmartDashboard.getNumber(smartDashboardKey, 0), 0.01);
+ assertEquals(1, spark.getSmartCurrentLimit());
+
+ spark.setTemperature(20);
+ spark.setSmartCurrentLimit(50);
+ MotorErrors.doReportSparkMaxTemp();
+ assertEquals(20, SmartDashboard.getNumber(smartDashboardKey, 0), 0.01);
+ assertEquals(1, spark.getSmartCurrentLimit());
+
+ assertEquals(0, errStream.size());
+ } catch(Exception e) {
  assumeNoException(e);
  }
  }
 
+ // Blocks the Lib199Subsystem's async thread until closed
+ private AutoCloseable blockAsyncPeriodic() {
+ AtomicBoolean block = new AtomicBoolean(true);
+ Object lock = new Object();
+ Lib199Subsystem.registerAsyncPeriodic(() -> {
+ synchronized(lock) {
+ while(block.get()) {
+ try {
+ lock.wait();
+ } catch(InterruptedException e) {
+ assumeNoException(e);
+ }
+ }
+ }
+ });
+ return () -> {
+ block.set(false);
+ synchronized(lock) {
+ lock.notifyAll();
+ }
+ };
+ }
+
 }