From 97f367be187d4c06c795af03df4220247319c773 Mon Sep 17 00:00:00 2001
From: Vignesh Balasubramaniam <vigneshb.hp@gmail.com>
Date: Thu, 20 Feb 2025 19:29:32 -0600
Subject: [PATCH] Fix concurrent variable editing

---
 .../lasarobotics/hardware/PurpleManager.java  | 28 +++++++++++++++----
 .../hardware/revrobotics/Spark.java           |  4 +--
 2 files changed, 24 insertions(+), 8 deletions(-)

diff --git a/src/main/java/org/lasarobotics/hardware/PurpleManager.java b/src/main/java/org/lasarobotics/hardware/PurpleManager.java
index d44a809..e72a65e 100644
--- a/src/main/java/org/lasarobotics/hardware/PurpleManager.java
+++ b/src/main/java/org/lasarobotics/hardware/PurpleManager.java
@@ -46,10 +46,10 @@
 public class PurpleManager {
   private static double GARBAGE_COLLECTION_SEC = 5.0;
   private static Map<LoggableHardware, ScheduledFuture<?>> m_hardware = new HashMap<>();
-  private static List<Monitorable> m_monitored = new ArrayList<>();
-  private static List<Runnable> m_callbacks = new ArrayList<>();
-  private static List<Runnable> m_simCallbacks = new ArrayList<>();
-  private static VisionSystemSim m_visionSim = new VisionSystemSim("PurpleManager");
+  private static volatile List<Monitorable> m_monitored = new ArrayList<>();
+  private static volatile List<Runnable> m_callbacks = new ArrayList<>();
+  private static volatile List<Runnable> m_simCallbacks = new ArrayList<>();
+  private static volatile VisionSystemSim m_visionSim = new VisionSystemSim("PurpleManager");
   private static Supplier<Pose2d> m_poseSupplier = null;
   private static Timer m_garbageTimer = new Timer();
   private static ScheduledExecutorService m_inputUpdater = Executors.newSingleThreadScheduledExecutor(new ThreadFactory() {
@@ -62,6 +62,16 @@ public Thread newThread(Runnable r) {
         return t;
     }
   });
+  private static ScheduledExecutorService m_healthMonitor = Executors.newSingleThreadScheduledExecutor(new ThreadFactory() {
+    private ThreadFactory defaultThreadFactory = Executors.defaultThreadFactory();
+
+    @Override
+    public Thread newThread(Runnable r) {
+        Thread t = defaultThreadFactory.newThread(r);
+        t.setPriority(Thread.MAX_PRIORITY - 2); // Input thread and pose estimation are higher priority
+        return t;
+    }
+  });
 
   /**
    * Monitor health of components
@@ -200,6 +210,13 @@ public static void initialize(LoggedRobot robot,
       SignalLogger.start();
     }
 
+    // Start health monitor thread
+    m_healthMonitor.scheduleAtFixedRate(PurpleManager::monitorHealth,
+      0,
+      (long)GlobalConstants.ROBOT_LOOP_HZ.asPeriod().in(Units.Microseconds),
+      java.util.concurrent.TimeUnit.MICROSECONDS
+    );
+
     // Start logging! No more data receivers, replay sources, or metadata values may be added.
     Logger.start();
   }
@@ -282,8 +299,7 @@ public static void update() {
     // Run garbage collector regularly
     if (m_garbageTimer.advanceIfElapsed(GARBAGE_COLLECTION_SEC)) System.gc();
 
-    // Monitor health and run periodic logic
-    monitorHealth();
+    // Run periodic logic
     m_hardware.keySet().stream().forEach((device) -> device.periodic());
     m_callbacks.stream().forEach(Runnable::run);
 
diff --git a/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java b/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java
index 9d08766..569131e 100644
--- a/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java
+++ b/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java
@@ -126,8 +126,8 @@ public static class SparkInputs {
   }
 
   private static final String LOG_TAG = "Spark";
-  private static final int CAN_TIMEOUT_MS = 2000;
-  private static final int MAX_ATTEMPTS = 20;
+  private static final int CAN_TIMEOUT_MS = 200;
+  private static final int MAX_ATTEMPTS = 50;
   private static final int SPARK_MAX_MEASUREMENT_PERIOD = 16;
   private static final int SPARK_FLEX_MEASUREMENT_PERIOD = 32;
   private static final int SPARK_MAX_AVERAGE_DEPTH = 2;