diff --git a/src/main/java/org/carlmontrobotics/lib199/Lib199Subsystem.java b/src/main/java/org/carlmontrobotics/lib199/Lib199Subsystem.java index c8e242e..e9201e4 100644 --- a/src/main/java/org/carlmontrobotics/lib199/Lib199Subsystem.java +++ b/src/main/java/org/carlmontrobotics/lib199/Lib199Subsystem.java @@ -11,32 +11,13 @@ public class Lib199Subsystem implements Subsystem { private static final Lib199Subsystem INSTANCE = new Lib199Subsystem(); private static final CopyOnWriteArrayList periodicMethods = new CopyOnWriteArrayList<>(); private static final CopyOnWriteArrayList periodicSimulationMethods = new CopyOnWriteArrayList<>(); - private static final CopyOnWriteArrayList asyncPeriodicMethods = new CopyOnWriteArrayList<>(); - private static final CopyOnWriteArrayList asyncPeriodicSimulationMethods = new CopyOnWriteArrayList<>(); private static final Consumer RUN_RUNNABLE = Runnable::run; - private static final Thread asyncPeriodicThread; - + @Deprecated public static final long asyncSleepTime = 20; static { ensureRegistered(); - - asyncPeriodicThread = new Thread(() -> { - while(true) { - try { - INSTANCE.asyncPeriodic(); - try { - Thread.sleep(asyncSleepTime); - } catch(InterruptedException e) {} - } catch (Exception ex) { - System.err.println("Lib199 error running ayncPeriodic() methods: " + ex); - ex.printStackTrace(System.err); - } - } - }); - asyncPeriodicThread.setDaemon(true); - asyncPeriodicThread.start(); } private static boolean registered = false; @@ -66,12 +47,22 @@ public static void registerSimulationPeriodic(Runnable method) { periodicSimulationMethods.add(method); } + @Deprecated + /** + * @deprecated Use registerPeriodic + * @param method + */ public static void registerAsyncPeriodic(Runnable method) { - asyncPeriodicMethods.add(method); + registerPeriodic(method); } + @Deprecated + /** + * @deprecated Use registerSimulationPeriodic + * @param method + */ public static void registerAsyncSimulationPeriodic(Runnable method) { - if(RobotBase.isSimulation()) asyncPeriodicSimulationMethods.add(method); + registerSimulationPeriodic(method); } @Override @@ -84,22 +75,11 @@ public void simulationPeriodic() { periodicSimulationMethods.forEach(RUN_RUNNABLE); } - public synchronized void asyncPeriodic() { - asyncPeriodicMethods.forEach(RUN_RUNNABLE); - asyncPeriodicSimulationMethods.forEach(RUN_RUNNABLE); - } - + @Deprecated /** - * Unregisters all Runnables registered with registerAsyncPeriodic() and - * registerAsyncSimulationPeriodic(). Blocks until any currently registered - * Runnables have finished running. This is particularly useful for ensuring - * that Runnables registered in one test don't interfere with other tests. + * No longer does anything. */ - public static void unregisterAllAsync() { - synchronized (INSTANCE) { - asyncPeriodicMethods.clear(); - asyncPeriodicSimulationMethods.clear(); - } + public synchronized void asyncPeriodic() { } private Lib199Subsystem() {} diff --git a/src/main/java/org/carlmontrobotics/lib199/MotorErrors.java b/src/main/java/org/carlmontrobotics/lib199/MotorErrors.java index 6cacbe1..5d5a065 100644 --- a/src/main/java/org/carlmontrobotics/lib199/MotorErrors.java +++ b/src/main/java/org/carlmontrobotics/lib199/MotorErrors.java @@ -1,7 +1,9 @@ package org.carlmontrobotics.lib199; import java.util.Arrays; +import java.util.Map; import java.util.concurrent.ConcurrentHashMap; +import java.util.concurrent.ConcurrentSkipListMap; import com.ctre.phoenix.ErrorCode; import com.revrobotics.CANSparkBase; @@ -14,17 +16,23 @@ public final class MotorErrors { - 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 Map temperatureSparks = new ConcurrentSkipListMap<>(); + private static final Map sparkTemperatureLimits = new ConcurrentHashMap<>(); + private static final Map overheatedSparks = new ConcurrentHashMap<>(); + private static final Map flags = new ConcurrentSkipListMap<>( + (spark1, spark2) -> (spark1.getDeviceId() - spark2.getDeviceId())); + private static final Map stickyFlags = new ConcurrentSkipListMap<>( + (spark1, spark2) -> (spark1.getDeviceId() - spark2.getDeviceId())); public static final int kOverheatTripCount = 5; static { - Lib199Subsystem.registerAsyncPeriodic(MotorErrors::doReportSparkTemp); - Lib199Subsystem.registerAsyncPeriodic(MotorErrors::printSparkErrorMessages); + Lib199Subsystem.registerPeriodic(() -> { + MotorErrors.reportNextNSparkTemps(2); + }); + Lib199Subsystem.registerPeriodic(() -> { + MotorErrors.reportNextNSparkErrors(2); + }); } public static void reportError(ErrorCode error) { @@ -109,6 +117,14 @@ public static void printSparkErrorMessages() { flags.keySet().forEach(MotorErrors::checkSparkErrors); } + private static int lastSparkErrorIndexReported = 0; + + static void reportNextNSparkErrors(int n) { + flags.keySet().stream().skip(lastSparkErrorIndexReported).limit(n) + .forEach(MotorErrors::checkSparkErrors); + lastSparkErrorIndexReported = (lastSparkErrorIndexReported + n) % flags.size(); + } + public static CANSparkMax createDummySparkMax() { return DummySparkMaxAnswer.DUMMY_SPARK_MAX; } @@ -142,37 +158,52 @@ public static void doReportSparkMaxTemp() { } public static void doReportSparkTemp() { - temperatureSparks.forEach((port, spark) -> { - double temp = spark.getMotorTemperature(); - double limit = sparkTemperatureLimits.get(port); - int numTrips = overheatedSparks.get(port); - 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) { - overheatedSparks.put(port, ++numTrips); - } else { - overheatedSparks.put(port, 0); - } + temperatureSparks.forEach(MotorErrors::reportSparkTemp); + } + + private static int lastSparkTempIndexReported = 0; + + static void reportNextNSparkTemps(int n) { + temperatureSparks.entrySet().stream().skip(lastSparkTempIndexReported).limit(n) + .forEach((entry) -> reportSparkTemp(entry.getKey(), entry.getValue())); + lastSparkTempIndexReported = (lastSparkTempIndexReported + n) % temperatureSparks.size(); + } + + private static void reportSparkTemp(int port, CANSparkBase spark) { + double temp = spark.getMotorTemperature(); + double limit = sparkTemperatureLimits.get(port); + int numTrips = overheatedSparks.get(port); + 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) { + 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(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 is operating at " + temp + " degrees Celsius! It will be disabled until the robot code is restarted."); - } - spark.setSmartCurrentLimit(1); + // 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 (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 is operating at " + temp + + " degrees Celsius! It will be disabled until the robot code is restarted."); } - }); + spark.setSmartCurrentLimit(1); + } } private MotorErrors() {} diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkBase.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkBase.java index 36ef496..afc45e2 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)}. + * registers this class's {@link #run()} method to be called via {@link Lib199Subsystem#registerSimulationPeriodic(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 @@ -72,7 +72,7 @@ public REVLibError setInverted(boolean inverted) { controllers.put(port, this); - Lib199Subsystem.registerAsyncSimulationPeriodic(this); + Lib199Subsystem.registerSimulationPeriodic(this); } @Override diff --git a/src/test/java/org/carlmontrobotics/lib199/MotorErrorsTest.java b/src/test/java/org/carlmontrobotics/lib199/MotorErrorsTest.java index 956541c..bdb0685 100644 --- a/src/test/java/org/carlmontrobotics/lib199/MotorErrorsTest.java +++ b/src/test/java/org/carlmontrobotics/lib199/MotorErrorsTest.java @@ -171,11 +171,23 @@ private void doTestReportSparkMaxTemp(int id) { String smartDashboardKey = "Port " + id + " Spark Max Temp"; MotorErrors.reportSparkMaxTemp((CANSparkMax)spark, 40); - try(AutoCloseable asyncBlock = blockAsyncPeriodic()) { - spark.setTemperature(20); + 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()); + + if(MotorErrors.kOverheatTripCount > 1) { + spark.setTemperature(51); spark.setSmartCurrentLimit(50); MotorErrors.doReportSparkMaxTemp(); - assertEquals(20, SmartDashboard.getNumber(smartDashboardKey, 0), 0.01); + assertEquals(51, SmartDashboard.getNumber(smartDashboardKey, 0), 0.01); assertEquals(50, spark.getSmartCurrentLimit()); spark.setTemperature(20); @@ -183,82 +195,35 @@ private void doTestReportSparkMaxTemp(int id) { MotorErrors.doReportSparkMaxTemp(); assertEquals(20, SmartDashboard.getNumber(smartDashboardKey, 0), 0.01); assertEquals(50, spark.getSmartCurrentLimit()); + } - 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()); - 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); - } - - 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()); + assertNotEquals(0, errStream.size()); + errStream.reset(); - assertEquals(0, errStream.size()); - } catch(Exception e) { - assumeNoException(e); - } - } + spark.setTemperature(51); + spark.setSmartCurrentLimit(50); + MotorErrors.doReportSparkMaxTemp(); + assertEquals(51, SmartDashboard.getNumber(smartDashboardKey, 0), 0.01); + assertEquals(1, spark.getSmartCurrentLimit()); - // Blocks the Lib199Subsystem's async thread until closed - private AutoCloseable blockAsyncPeriodic() { - AtomicBoolean block = new AtomicBoolean(true); - Object lock = new Object(); - CountDownLatch latch = new CountDownLatch(1); - Lib199Subsystem.registerAsyncPeriodic(() -> { - synchronized(lock) { - latch.countDown(); // Signal that we've started blocking - while(block.get()) { // Block until released - try { - lock.wait(); // Wait for the block to be released - } catch(InterruptedException e) { - assumeNoException(e); - } - } - } - }); - try { - latch.await(); // Wait for the async thread to start blocking - } catch(InterruptedException e) { - assumeNoException(e); - } - return () -> { - block.set(false); // Release the block - synchronized(lock) { - lock.notifyAll(); // Notify the async thread that the block has been released - } - }; - } + spark.setTemperature(20); + spark.setSmartCurrentLimit(50); + MotorErrors.doReportSparkMaxTemp(); + assertEquals(20, SmartDashboard.getNumber(smartDashboardKey, 0), 0.01); + assertEquals(1, spark.getSmartCurrentLimit()); + assertEquals(0, errStream.size()); + } } diff --git a/src/test/java/org/carlmontrobotics/lib199/testUtils/TestRules.java b/src/test/java/org/carlmontrobotics/lib199/testUtils/TestRules.java index ef6d119..98897dd 100644 --- a/src/test/java/org/carlmontrobotics/lib199/testUtils/TestRules.java +++ b/src/test/java/org/carlmontrobotics/lib199/testUtils/TestRules.java @@ -31,13 +31,8 @@ public Statement apply(Statement base, Description description) { return new Statement(){ @Override public void evaluate() throws Throwable { - // Ensure there are no async periodic things running before - // we reset the SimDeviceData so that they don't try to - // touch devices/data after they have been removed. - Lib199Subsystem.unregisterAllAsync(); SimDeviceSim.resetData(); base.evaluate(); - Lib199Subsystem.unregisterAllAsync(); SimDeviceSim.resetData(); } };