Make lib199 periodic methods run asynchronously

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

@@ -1,21 +1,39 @@
 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> periodicMethods = new CopyOnWriteArrayList<>();
+    private static final CopyOnWriteArrayList<Runnable> periodicSimulationMethods = new CopyOnWriteArrayList<>();
+    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 +48,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 +79,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

@@ -6,9 +6,8 @@
 import java.lang.reflect.Modifier;
 import java.util.ArrayList;
 import java.util.Arrays;
-import java.util.Collections;
 import java.util.HashMap;
-import java.util.List;
+import java.util.concurrent.CopyOnWriteArrayList;
 import java.util.function.Consumer;
 import java.util.function.Predicate;
 import java.util.stream.Collectors;
@@ -21,7 +20,7 @@
 
 public final class Mocks {
 
-    private static final List<WeakReference<Object>> MOCKS = Collections.synchronizedList(new ArrayList<>());
+    private static final CopyOnWriteArrayList<WeakReference<Object>> MOCKS = new CopyOnWriteArrayList<>();
     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_REFERENCE_NOT_CLEARED = reference -> {
@@ -37,7 +36,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_REFERENCE_NOT_CLEARED));
+        Lib199Subsystem.registerAsyncPeriodic(() -> MOCKS.removeIf(CLEAR_INVOCATIONS_ON_REFERENCED_MOCK_IF_REFERENCE_NOT_CLEARED));
     }
 
     /**

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

@@ -1,8 +1,8 @@
 package org.carlmontrobotics.lib199;
 
-import java.util.ArrayList;
 import java.util.Arrays;
-import java.util.HashMap;
+import java.util.concurrent.ConcurrentHashMap;
+import java.util.concurrent.CopyOnWriteArrayList;
 
 import com.ctre.phoenix.ErrorCode;
 import com.revrobotics.CANSparkMax;
@@ -13,14 +13,14 @@
 
 public final class MotorErrors {
 
-    private static final HashMap<Integer, CANSparkMax> temperatureSparks = new HashMap<>();
-    private static final HashMap<Integer, Integer> sparkTemperatureLimits = new HashMap<>();
-    private static final ArrayList<Integer> overheatedSparks = new ArrayList<>();
-    private static final HashMap<CANSparkMax, Short> flags = new HashMap<>();
-    private static final HashMap<CANSparkMax, Short> stickyFlags = new HashMap<>();
+    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<CANSparkMax, Short> flags = new ConcurrentHashMap<>();
+    private static final ConcurrentHashMap<CANSparkMax, Short> stickyFlags = new ConcurrentHashMap<>();
 
     static {
-        Lib199Subsystem.registerPeriodic(MotorErrors::doReportSparkMaxTemp);
+        Lib199Subsystem.registerAsyncPeriodic(MotorErrors::doReportSparkMaxTemp);
     }
 
     public static void reportError(ErrorCode error) {

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

@@ -1,7 +1,7 @@
 package org.carlmontrobotics.lib199.sim;
 
-import java.util.ArrayList;
-import java.util.HashMap;
+import java.util.concurrent.ConcurrentHashMap;
+import java.util.concurrent.CopyOnWriteArrayList;
 
 import com.ctre.phoenix.motorcontrol.ControlMode;
 import com.ctre.phoenix.motorcontrol.IMotorController;
@@ -15,7 +15,7 @@ abstract class MockPhoenixController implements AutoCloseable {
     // CAN ports should be separate from PWM ports
     protected PWMMotorController motorPWM;
     // Since we need to keep a record of all the motor's followers
-    protected static HashMap<Integer, ArrayList<PWMMotorController>> followMap = new HashMap<>();
+    protected static ConcurrentHashMap<Integer, CopyOnWriteArrayList<PWMMotorController>> followMap = new ConcurrentHashMap<>();
 
     public MockPhoenixController(int portPWM) {
         this.portPWM = portPWM;
@@ -36,7 +36,7 @@ public double get() {
 
     public void follow(IMotorController leader) {
         if (!followMap.containsKey(leader.getDeviceID())) {
-            ArrayList<PWMMotorController> arr = new ArrayList<PWMMotorController>();
+            CopyOnWriteArrayList<PWMMotorController> arr = new CopyOnWriteArrayList<PWMMotorController>();
             arr.add(motorPWM);
             followMap.put(leader.getDeviceID(), arr);
         } else {

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

@@ -1,23 +1,23 @@
 package org.carlmontrobotics.lib199.sim;
 
-import java.util.ArrayList;
-import java.util.HashMap;
+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.REVLibError;
-import com.revrobotics.RelativeEncoder;
-import com.revrobotics.SparkMaxPIDController;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkMax.ExternalFollower;
 import com.revrobotics.CANSparkMax.IdleMode;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
-
-import org.carlmontrobotics.lib199.DummySparkMaxAnswer;
-import org.carlmontrobotics.lib199.Mocks;
-import org.carlmontrobotics.lib199.REVLibErrorAnswer;
+import com.revrobotics.REVLibError;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.SparkMaxPIDController;
 
 import edu.wpi.first.hal.SimDevice;
-import edu.wpi.first.hal.SimDouble;
 import edu.wpi.first.hal.SimDevice.Direction;
+import edu.wpi.first.hal.SimDouble;
 
 public class MockSparkMax {
     // Assign the CAN port to a PWM port so it works with the simulator. Not a fan
@@ -30,7 +30,7 @@ public class MockSparkMax {
     private SparkMaxPIDController pidController;
     private boolean isInverted;
     // Since we need to keep a record of all the motor's followers
-    private static HashMap<Integer, ArrayList<SimDouble>> followMap = new HashMap<>();
+    private static ConcurrentHashMap<Integer, CopyOnWriteArrayList<SimDouble>> followMap = new ConcurrentHashMap<>();
 
     public MockSparkMax(int port, MotorType type) {
         this.port = port;
@@ -67,7 +67,7 @@ public REVLibError follow(ExternalFollower leader, int deviceID) {
 
     public REVLibError follow(ExternalFollower leader, int deviceID, boolean invert) {
         if (!followMap.containsKey(deviceID)) {
-            followMap.put(deviceID, new ArrayList<SimDouble>());
+            followMap.put(deviceID, new CopyOnWriteArrayList<SimDouble>());
         }
         followMap.get(deviceID).add(speed);
         return REVLibError.kOk;

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);
+        }
+    }
+
 }