make lib199 periodic methods run asynchronously

Author: CoolSpy3

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

@@ -1,21 +1,40 @@
 package org.carlmontrobotics.lib199;
 
 import java.util.ArrayList;
+import java.util.concurrent.CopyOnWriteArrayList;
 import java.util.function.Consumer;
 
+import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj2.command.Subsystem;
 
 public class Lib199Subsystem implements Subsystem {
 
     private static final Lib199Subsystem INSTANCE = new Lib199Subsystem();
     private static final ArrayList<Runnable> periodicMethods = new ArrayList<>();
     private static final ArrayList<Runnable> periodicSimulationMethods = new ArrayList<>();
+    private static final CopyOnWriteArrayList<Runnable> asyncPeriodicMethods = new CopyOnWriteArrayList<>();
+    private static final CopyOnWriteArrayList<Runnable> asyncPeriodicSimulationMethods = new CopyOnWriteArrayList<>();
     private static final Consumer<Runnable> RUN_RUNNABLE = Runnable::run;
 
+    private static final Thread asyncPeriodicThread;
+
+    public static final long asyncSleepTime = 20;
+
     static {
         ensureRegistered();
+
+        asyncPeriodicThread = new Thread(() -> {
+            while(true) {
+                INSTANCE.asyncPeriodic();
+                try {
+                    Thread.sleep(asyncSleepTime);
+                } catch(InterruptedException e) {}
+            }
+        });
+        asyncPeriodicThread.setDaemon(true);
+        asyncPeriodicThread.start();
     }
-    
+
     private static boolean registered = false;
 
     private static void ensureRegistered() {
@@ -30,10 +49,27 @@ public static void registerPeriodic(Runnable method) {
         periodicMethods.add(method);
     }
 
+    @Deprecated
+    /**
+     * @deprecated Use registerSimulationPeriodic
+     * @param method
+     */
     public static void simulationPeriodic(Runnable method) {
+        registerSimulationPeriodic(method);
+    }
+
+    public static void registerSimulationPeriodic(Runnable method) {
         periodicSimulationMethods.add(method);
     }
 
+    public static void registerAsyncPeriodic(Runnable method) {
+        asyncPeriodicMethods.add(method);
+    }
+
+    public static void registerAsyncSimulationPeriodic(Runnable method) {
+        if(RobotBase.isSimulation()) asyncPeriodicSimulationMethods.add(method);
+    }
+
     @Override
     public void periodic() {
         periodicMethods.forEach(RUN_RUNNABLE);
@@ -44,6 +80,11 @@ public void simulationPeriodic() {
         periodicSimulationMethods.forEach(RUN_RUNNABLE);
     }
 
+    public void asyncPeriodic() {
+        asyncPeriodicMethods.forEach(RUN_RUNNABLE);
+        asyncPeriodicSimulationMethods.forEach(RUN_RUNNABLE);
+    }
+
     private Lib199Subsystem() {}
 
 }

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

@@ -24,7 +24,7 @@ public final class Mocks {
     private static final List<WeakReference<Object>> MOCKS = Collections.synchronizedList(new ArrayList<>());
     private static final Predicate<WeakReference<?>> IS_REFERENCE_CLEARED = reference -> reference.get() == null;
     private static final Consumer<WeakReference<Object>> CLEAR_INVOCATIONS_ON_REFERENCED_MOCK = reference -> Mockito.clearInvocations(reference.get());
-    private static final Predicate<WeakReference<Object>> CLEAR_INVOCATIONS_ON_REFERENCED_MOCK_IF_REFERNCE_NOT_CLEARED = reference -> {
+    private static final Predicate<WeakReference<Object>> CLEAR_INVOCATIONS_ON_REFERENCED_MOCK_IF_REFERENCE_NOT_CLEARED = reference -> {
         if(IS_REFERENCE_CLEARED.test(reference)) return true;
         CLEAR_INVOCATIONS_ON_REFERENCED_MOCK.accept(reference);
         return false;
@@ -37,7 +37,7 @@ public final class Mocks {
         // 2) Garbage collected references are removed
         // 3) Mock is garbage collected
         // 4) Mock invocations are cleared -> throws NullPointerException
-        Lib199Subsystem.registerPeriodic(() -> MOCKS.removeIf(CLEAR_INVOCATIONS_ON_REFERENCED_MOCK_IF_REFERNCE_NOT_CLEARED));
+        Lib199Subsystem.registerAsyncPeriodic(() -> MOCKS.removeIf(CLEAR_INVOCATIONS_ON_REFERENCED_MOCK_IF_REFERENCE_NOT_CLEARED));
     }
 
     /**

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

@@ -20,7 +20,7 @@ public final class MotorErrors {
     private static final HashMap<CANSparkMax, Short> stickyFlags = new HashMap<>();
 
     static {
-        Lib199Subsystem.registerPeriodic(MotorErrors::doReportSparkMaxTemp);
+        Lib199Subsystem.registerAsyncPeriodic(MotorErrors::doReportSparkMaxTemp);
     }
 
     public static void reportError(ErrorCode error) {

src/main/java/org/carlmontrobotics/lib199/sim/MockedCANCoder.java

@@ -29,7 +29,7 @@ public MockedCANCoder(CANCoder canCoder) {
         position = device.createDouble("count", Direction.kInput, 0);
         gearing = device.createDouble("gearing", Direction.kOutput, 1);
         sim = canCoder.getSimCollection();
-        Lib199Subsystem.registerPeriodic(this::update);
+        Lib199Subsystem.registerAsyncSimulationPeriodic(this::update);
         sims.put(port, this);
     }
 

src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkEncoder.java

@@ -30,7 +30,7 @@ public MockedSparkEncoder(int id) {
         count = device.createDouble("count", Direction.kInput, 0);
         gearing = device.createDouble("gearing", Direction.kOutput, 1);
         sims.put(id, this);
-        Lib199Subsystem.registerPeriodic(this);
+        Lib199Subsystem.registerAsyncPeriodic(this);
     }
 
     public double getPosition() {

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

@@ -25,7 +25,7 @@ public void testPeriodic() {
     public void testSimulationPeriodic() {
         assumeTrue(RobotBase.isSimulation());
         AtomicInteger counter = new AtomicInteger(0);
-        Lib199Subsystem.registerPeriodic(() -> counter.addAndGet(1));
+        Lib199Subsystem.registerSimulationPeriodic(() -> counter.addAndGet(1));
         assertEquals("Simulation periodic method called before CommandScheduler.run", 0, counter.get());
         CommandScheduler.getInstance().run();
         assertEquals("Simulation periodic method called more than once or not at all", 1, counter.get());

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

@@ -2,6 +2,7 @@
 
 import static org.junit.Assert.assertEquals;
 import static org.junit.Assert.assertNotEquals;
+import static org.junit.Assume.assumeNoException;
 
 import com.ctre.phoenix.ErrorCode;
 import com.revrobotics.REVLibError;
@@ -12,7 +13,6 @@
 import org.junit.Test;
 
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
 public class MotorErrorsTest extends ErrStreamTest {
 
@@ -77,6 +77,16 @@ public int getDeviceId() {
 
     }
 
+    private static final Object asyncPeriodicNotifier = new Object();
+
+    static {
+        Lib199Subsystem.registerAsyncPeriodic(() -> {
+            synchronized(asyncPeriodicNotifier) {
+                asyncPeriodicNotifier.notifyAll();
+            }
+        });
+    }
+
     @Test
     public void testOkErrors() {
         errStream.reset();
@@ -165,30 +175,43 @@ private void doTestReportSparkMaxTemp(int id) {
         MotorErrors.reportSparkMaxTemp((CANSparkMax)spark, 40);
         spark.setSmartCurrentLimit(50);
         spark.setTemperature(20);
-        CommandScheduler.getInstance().run();
+        runAsyncPeriodic();
         String smartDashboardKey = "Port " + id + " Spark Max Temp";
         assertEquals(20, SmartDashboard.getNumber(smartDashboardKey, 0), 0.01);
         assertEquals(50, spark.getSmartCurrentLimit());
         spark.setTemperature(20);
-        CommandScheduler.getInstance().run();
+        runAsyncPeriodic();
         assertEquals(20, SmartDashboard.getNumber(smartDashboardKey, 0), 0.01);
         assertEquals(50, spark.getSmartCurrentLimit());
         assertEquals(0, errStream.size());
         spark.setTemperature(40);
-        CommandScheduler.getInstance().run();
+        runAsyncPeriodic();
         assertEquals(40, SmartDashboard.getNumber(smartDashboardKey, 0), 0.01);
         assertEquals(1, spark.getSmartCurrentLimit());
         assertNotEquals(0, errStream.size());
         errStream.reset();
         spark.setTemperature(50);
-        CommandScheduler.getInstance().run();
+        runAsyncPeriodic();
         assertEquals(50, SmartDashboard.getNumber(smartDashboardKey, 0), 0.01);
         assertEquals(1, spark.getSmartCurrentLimit());
         spark.setTemperature(20);
-        CommandScheduler.getInstance().run();
+        runAsyncPeriodic();
         assertEquals(20, SmartDashboard.getNumber(smartDashboardKey, 0), 0.01);
         assertEquals(1, spark.getSmartCurrentLimit());
         assertEquals(0, errStream.size());
     }
 
+    // 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();
+            }
+        } catch(InterruptedException e) {
+            assumeNoException(e);
+        }
+    }
+
 }