[hal, wpilib] New DS thread model and implementation (#3787)

The current DS thread model has some pretty major issues. It makes it difficult to know if all data is from the same remote packet, and if the data changes while the robot loop is running. Additionally, the DS thread is used for a few other things (MotorSafety and State Tracking for EducationalRobot). This also makes sim difficult, as user code has to wait for the thread to know it has new data.

This change completely rethinks how threading works in the driver station model.

First, the DS HAL system receives a new data callback, either from Netcomm or DriverStationSim. Inside the context of this callback, all the low latency data is read and put into a cache. Doing some investigation on the robot side, this is perfectly safe to do, and also ensures a ds packet will not be parsed before we finish reading the current packet data.

After all data is read, the cache is swapped with a 2nd buffer. This buffer just stores the data, none of the HAL DS calls read from this buffer. An event is then fired, stating there is new data ready to go.

Robot code calls HAL_UpdateDSData(). This swaps the 2nd buffer with a 3rd buffer, which always contains the current data. This data will not be updated until HAL_UpdateDSData is called again. Which solves the state problem.

The high level driver station classes have. an updateData() call, which calls HAL_UpdateDSData, and then update button state variables, then data log and update the NT FMS data table (Java also caches across the JNI boundary here, but that could trivially be removed). An extra event provider is provided, allowing other threads to know when this call has been completed.

IterativeRobotBase calls DS.updateData() at the beginning of each loop, and only once per loop. This means all commands will always have the same state.

All of this means there is no longer a DS thread. Everything happens synchronously. This means Sim and testing is easier, as you can just call DriverStationSim.NotifyNewData(), and then DriverStation.UpdateData(), and you can guarantee that all the DriverStation.*** data is up to date.

As for Motor Safety and Educational Robot State Handling, those can all be handled by their own threads. The Educational Thread only needs to run under EducationalRobot, and MotorSafety will only be started if there is a motor safety object enabled.

wpilibj/src/main/java/edu/wpi/first/wpilibj/DSControlWord.java

@@ -16,12 +16,12 @@ public class DSControlWord {
    * <p>Upon construction, the current Driver Station control word is read and stored internally.
    */
   public DSControlWord() {
-    update();
+    refresh();
   }
 
   /** Update internal Driver Station control word. */
-  public void update() {
-    DriverStation.updateControlWordFromCache(m_controlWord);
+  public void refresh() {
+    DriverStation.refreshControlWordFromCache(m_controlWord);
   }
 
   /**

wpilibj/src/main/java/edu/wpi/first/wpilibj/DataLogManager.java

@@ -6,6 +6,7 @@ package edu.wpi.first.wpilibj;
 
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.util.WPIUtilJNI;
+import edu.wpi.first.util.concurrent.Event;
 import edu.wpi.first.util.datalog.DataLog;
 import edu.wpi.first.util.datalog.IntegerLogEntry;
 import edu.wpi.first.util.datalog.StringLogEntry;
@@ -279,12 +280,19 @@ public final class DataLogManager {
         new IntegerLogEntry(
             m_log, "systemTime", "{\"source\":\"DataLogManager\",\"format\":\"time_t_us\"}");
 
+    Event newDataEvent = new Event();
+    DriverStation.provideRefreshedDataEventHandle(newDataEvent.getHandle());
     while (!Thread.interrupted()) {
-      boolean newData = DriverStation.waitForData(0.25);
+      boolean timedOut;
+      try {
+        timedOut = WPIUtilJNI.waitForObjectTimeout(newDataEvent.getHandle(), 0.25);
+      } catch (InterruptedException e) {
+        break;
+      }
       if (Thread.interrupted()) {
         break;
       }
-      if (!newData) {
+      if (timedOut) {
         timeoutCount++;
         // pause logging after being disconnected for 10 seconds
         if (timeoutCount > 40 && !paused) {
@@ -371,5 +379,6 @@ public final class DataLogManager {
         sysTimeEntry.append(WPIUtilJNI.getSystemTime(), WPIUtilJNI.now());
       }
     }
+    newDataEvent.close();
   }
 }

wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java

@@ -6,6 +6,7 @@ package edu.wpi.first.wpilibj;
 
 import edu.wpi.first.hal.AllianceStationID;
 import edu.wpi.first.hal.ControlWord;
+import edu.wpi.first.hal.DriverStationJNI;
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.hal.MatchInfoData;
 import edu.wpi.first.networktables.BooleanPublisher;
@@ -13,6 +14,7 @@ import edu.wpi.first.networktables.IntegerPublisher;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.networktables.StringPublisher;
+import edu.wpi.first.util.EventVector;
 import edu.wpi.first.util.WPIUtilJNI;
 import edu.wpi.first.util.datalog.BooleanArrayLogEntry;
 import edu.wpi.first.util.datalog.BooleanLogEntry;
@@ -20,9 +22,6 @@ import edu.wpi.first.util.datalog.DataLog;
 import edu.wpi.first.util.datalog.FloatArrayLogEntry;
 import edu.wpi.first.util.datalog.IntegerArrayLogEntry;
 import java.nio.ByteBuffer;
-import java.util.concurrent.TimeUnit;
-import java.util.concurrent.locks.Condition;
-import java.util.concurrent.locks.Lock;
 import java.util.concurrent.locks.ReentrantLock;
 
 /** Provide access to the network communication data to / from the Driver Station. */
@@ -37,16 +36,25 @@ public final class DriverStation {
 
   private static class HALJoystickAxes {
     public float[] m_axes;
-    public short m_count;
+    public int m_count;
 
     HALJoystickAxes(int count) {
       m_axes = new float[count];
     }
   }
 
+  private static class HALJoystickAxesRaw {
+    public int[] m_axes;
+    public int m_count;
+
+    HALJoystickAxesRaw(int count) {
+      m_axes = new int[count];
+    }
+  }
+
   private static class HALJoystickPOVs {
     public short[] m_povs;
-    public short m_count;
+    public int m_count;
 
     HALJoystickPOVs(int count) {
       m_povs = new short[count];
@@ -73,15 +81,6 @@ public final class DriverStation {
   private static final double JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL = 1.0;
   private static double m_nextMessageTime;
 
-  private static class DriverStationTask implements Runnable {
-    DriverStationTask() {}
-
-    @Override
-    public void run() {
-      DriverStation.run();
-    }
-  } /* DriverStationTask */
-
   @SuppressWarnings("MemberName")
   private static class MatchDataSender {
     NetworkTable table;
@@ -126,7 +125,7 @@ public final class DriverStation {
     }
 
     private void sendMatchData() {
-      AllianceStationID allianceID = HAL.getAllianceStation();
+      AllianceStationID allianceID = DriverStationJNI.getAllianceStation();
       boolean isRedAlliance = false;
       int stationNumber = 1;
       switch (allianceID) {
@@ -172,7 +171,7 @@ public final class DriverStation {
       } finally {
         m_cacheDataMutex.unlock();
       }
-      currentControlWord = HAL.nativeGetControlWord();
+      currentControlWord = DriverStationJNI.nativeGetControlWord();
 
       if (oldIsRedAlliance != isRedAlliance) {
         alliance.set(isRedAlliance);
@@ -368,16 +367,22 @@ public final class DriverStation {
 
   // Joystick User Data
   private static HALJoystickAxes[] m_joystickAxes = new HALJoystickAxes[kJoystickPorts];
+  private static HALJoystickAxesRaw[] m_joystickAxesRaw = new HALJoystickAxesRaw[kJoystickPorts];
   private static HALJoystickPOVs[] m_joystickPOVs = new HALJoystickPOVs[kJoystickPorts];
   private static HALJoystickButtons[] m_joystickButtons = new HALJoystickButtons[kJoystickPorts];
   private static MatchInfoData m_matchInfo = new MatchInfoData();
+  private static ControlWord m_controlWord = new ControlWord();
+  private static EventVector m_refreshEvents = new EventVector();
 
   // Joystick Cached Data
   private static HALJoystickAxes[] m_joystickAxesCache = new HALJoystickAxes[kJoystickPorts];
+  private static HALJoystickAxesRaw[] m_joystickAxesRawCache =
+      new HALJoystickAxesRaw[kJoystickPorts];
   private static HALJoystickPOVs[] m_joystickPOVsCache = new HALJoystickPOVs[kJoystickPorts];
   private static HALJoystickButtons[] m_joystickButtonsCache =
       new HALJoystickButtons[kJoystickPorts];
   private static MatchInfoData m_matchInfoCache = new MatchInfoData();
+  private static ControlWord m_controlWordCache = new ControlWord();
 
   // Joystick button rising/falling edge flags
   private static int[] m_joystickButtonsPressed = new int[kJoystickPorts];
@@ -389,31 +394,10 @@ public final class DriverStation {
   private static final MatchDataSender m_matchDataSender;
   private static DataLogSender m_dataLogSender;
 
-  // Internal Driver Station thread
-  private static Thread m_thread;
-
-  private static volatile boolean m_threadKeepAlive = true;
-
   private static final ReentrantLock m_cacheDataMutex = new ReentrantLock();
 
-  private static final Lock m_waitForDataMutex;
-  private static final Condition m_waitForDataCond;
-  private static int m_waitForDataCount;
-  private static final ThreadLocal<Integer> m_lastCount = ThreadLocal.withInitial(() -> 0);
-
   private static boolean m_silenceJoystickWarning;
 
-  // Robot state status variables
-  private static boolean m_userInDisabled;
-  private static boolean m_userInAutonomous;
-  private static boolean m_userInTeleop;
-  private static boolean m_userInTest;
-
-  // Control word variables
-  private static final ReentrantLock m_controlWordMutex = new ReentrantLock();
-  private static final ControlWord m_controlWordCache;
-  private static long m_lastControlWordUpdate;
-
   /**
    * DriverStation constructor.
    *
@@ -424,42 +408,20 @@ public final class DriverStation {
 
   static {
     HAL.initialize(500, 0);
-    m_waitForDataCount = 0;
-    m_waitForDataMutex = new ReentrantLock();
-    m_waitForDataCond = m_waitForDataMutex.newCondition();
 
     for (int i = 0; i < kJoystickPorts; i++) {
       m_joystickButtons[i] = new HALJoystickButtons();
-      m_joystickAxes[i] = new HALJoystickAxes(HAL.kMaxJoystickAxes);
-      m_joystickPOVs[i] = new HALJoystickPOVs(HAL.kMaxJoystickPOVs);
+      m_joystickAxes[i] = new HALJoystickAxes(DriverStationJNI.kMaxJoystickAxes);
+      m_joystickAxesRaw[i] = new HALJoystickAxesRaw(DriverStationJNI.kMaxJoystickAxes);
+      m_joystickPOVs[i] = new HALJoystickPOVs(DriverStationJNI.kMaxJoystickPOVs);
 
       m_joystickButtonsCache[i] = new HALJoystickButtons();
-      m_joystickAxesCache[i] = new HALJoystickAxes(HAL.kMaxJoystickAxes);
-      m_joystickPOVsCache[i] = new HALJoystickPOVs(HAL.kMaxJoystickPOVs);
+      m_joystickAxesCache[i] = new HALJoystickAxes(DriverStationJNI.kMaxJoystickAxes);
+      m_joystickAxesRawCache[i] = new HALJoystickAxesRaw(DriverStationJNI.kMaxJoystickAxes);
+      m_joystickPOVsCache[i] = new HALJoystickPOVs(DriverStationJNI.kMaxJoystickPOVs);
     }
 
-    m_controlWordCache = new ControlWord();
-    m_lastControlWordUpdate = 0;
-
     m_matchDataSender = new MatchDataSender();
-
-    m_thread = new Thread(new DriverStationTask(), "FRCDriverStation");
-    m_thread.setPriority((Thread.NORM_PRIORITY + Thread.MAX_PRIORITY) / 2);
-
-    m_thread.start();
-  }
-
-  /** Kill the thread. */
-  public static synchronized void release() {
-    m_threadKeepAlive = false;
-    if (m_thread != null) {
-      try {
-        m_thread.join();
-      } catch (InterruptedException ex) {
-        Thread.currentThread().interrupt();
-      }
-      m_thread = null;
-    }
   }
 
   /**
@@ -537,7 +499,8 @@ public final class DriverStation {
         }
       }
     }
-    HAL.sendError(isError, code, false, error, locString, traceString.toString(), true);
+    DriverStationJNI.sendError(
+        isError, code, false, error, locString, traceString.toString(), true);
   }
 
   /**
@@ -666,7 +629,7 @@ public final class DriverStation {
     if (stick < 0 || stick >= kJoystickPorts) {
       throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
     }
-    if (axis < 0 || axis >= HAL.kMaxJoystickAxes) {
+    if (axis < 0 || axis >= DriverStationJNI.kMaxJoystickAxes) {
       throw new IllegalArgumentException("Joystick axis is out of range");
     }
 
@@ -699,7 +662,7 @@ public final class DriverStation {
     if (stick < 0 || stick >= kJoystickPorts) {
       throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
     }
-    if (pov < 0 || pov >= HAL.kMaxJoystickPOVs) {
+    if (pov < 0 || pov >= DriverStationJNI.kMaxJoystickPOVs) {
       throw new IllegalArgumentException("Joystick POV is out of range");
     }
 
@@ -760,10 +723,10 @@ public final class DriverStation {
   }
 
   /**
-   * Returns the number of POVs on a given joystick port.
+   * Returns the number of povs on a given joystick port.
    *
    * @param stick The joystick port number
-   * @return The number of POVs on the indicated joystick
+   * @return The number of povs on the indicated joystick
    */
   public static int getStickPOVCount(int stick) {
     if (stick < 0 || stick >= kJoystickPorts) {
@@ -808,7 +771,7 @@ public final class DriverStation {
       throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
     }
 
-    return HAL.getJoystickIsXbox((byte) stick) == 1;
+    return DriverStationJNI.getJoystickIsXbox((byte) stick) == 1;
   }
 
   /**
@@ -822,7 +785,7 @@ public final class DriverStation {
       throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
     }
 
-    return HAL.getJoystickType((byte) stick);
+    return DriverStationJNI.getJoystickType((byte) stick);
   }
 
   /**
@@ -836,7 +799,7 @@ public final class DriverStation {
       throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
     }
 
-    return HAL.getJoystickName((byte) stick);
+    return DriverStationJNI.getJoystickName((byte) stick);
   }
 
   /**
@@ -851,7 +814,7 @@ public final class DriverStation {
       throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
     }
 
-    return HAL.getJoystickAxisType((byte) stick, (byte) axis);
+    return DriverStationJNI.getJoystickAxisType((byte) stick, (byte) axis);
   }
 
   /**
@@ -875,12 +838,11 @@ public final class DriverStation {
    * @return True if the robot is enabled, false otherwise.
    */
   public static boolean isEnabled() {
-    m_controlWordMutex.lock();
+    m_cacheDataMutex.lock();
     try {
-      updateControlWord(false);
-      return m_controlWordCache.getEnabled() && m_controlWordCache.getDSAttached();
+      return m_controlWord.getEnabled() && m_controlWord.getDSAttached();
     } finally {
-      m_controlWordMutex.unlock();
+      m_cacheDataMutex.unlock();
     }
   }
 
@@ -899,12 +861,11 @@ public final class DriverStation {
    * @return True if the robot is e-stopped, false otherwise.
    */
   public static boolean isEStopped() {
-    m_controlWordMutex.lock();
+    m_cacheDataMutex.lock();
     try {
-      updateControlWord(false);
-      return m_controlWordCache.getEStop();
+      return m_controlWord.getEStop();
     } finally {
-      m_controlWordMutex.unlock();
+      m_cacheDataMutex.unlock();
     }
   }
 
@@ -915,12 +876,11 @@ public final class DriverStation {
    * @return True if autonomous mode should be enabled, false otherwise.
    */
   public static boolean isAutonomous() {
-    m_controlWordMutex.lock();
+    m_cacheDataMutex.lock();
     try {
-      updateControlWord(false);
-      return m_controlWordCache.getAutonomous();
+      return m_controlWord.getAutonomous();
     } finally {
-      m_controlWordMutex.unlock();
+      m_cacheDataMutex.unlock();
     }
   }
 
@@ -931,12 +891,11 @@ public final class DriverStation {
    * @return True if autonomous should be set and the robot should be enabled.
    */
   public static boolean isAutonomousEnabled() {
-    m_controlWordMutex.lock();
+    m_cacheDataMutex.lock();
     try {
-      updateControlWord(false);
-      return m_controlWordCache.getAutonomous() && m_controlWordCache.getEnabled();
+      return m_controlWord.getAutonomous() && m_controlWord.getEnabled();
     } finally {
-      m_controlWordMutex.unlock();
+      m_cacheDataMutex.unlock();
     }
   }
 
@@ -957,14 +916,13 @@ public final class DriverStation {
    * @return True if operator-controlled mode should be set and the robot should be enabled.
    */
   public static boolean isTeleopEnabled() {
-    m_controlWordMutex.lock();
+    m_cacheDataMutex.lock();
     try {
-      updateControlWord(false);
-      return !m_controlWordCache.getAutonomous()
-          && !m_controlWordCache.getTest()
-          && m_controlWordCache.getEnabled();
+      return !m_controlWord.getAutonomous()
+          && !m_controlWord.getTest()
+          && m_controlWord.getEnabled();
     } finally {
-      m_controlWordMutex.unlock();
+      m_cacheDataMutex.unlock();
     }
   }
 
@@ -975,12 +933,11 @@ public final class DriverStation {
    * @return True if test mode should be enabled, false otherwise.
    */
   public static boolean isTest() {
-    m_controlWordMutex.lock();
+    m_cacheDataMutex.lock();
     try {
-      updateControlWord(false);
-      return m_controlWordCache.getTest();
+      return m_controlWord.getTest();
     } finally {
-      m_controlWordMutex.unlock();
+      m_cacheDataMutex.unlock();
     }
   }
 
@@ -990,47 +947,25 @@ public final class DriverStation {
    * @return True if Driver Station is attached, false otherwise.
    */
   public static boolean isDSAttached() {
-    m_controlWordMutex.lock();
+    m_cacheDataMutex.lock();
     try {
-      updateControlWord(false);
-      return m_controlWordCache.getDSAttached();
+      return m_controlWord.getDSAttached();
     } finally {
-      m_controlWordMutex.unlock();
+      m_cacheDataMutex.unlock();
     }
   }
 
-  /**
-   * Gets if a new control packet from the driver station arrived since the last time this function
-   * was called.
-   *
-   * @return True if the control data has been updated since the last call.
-   */
-  public static boolean isNewControlData() {
-    m_waitForDataMutex.lock();
-    try {
-      int currentCount = m_waitForDataCount;
-      if (m_lastCount.get() != currentCount) {
-        m_lastCount.set(currentCount);
-        return true;
-      }
-    } finally {
-      m_waitForDataMutex.unlock();
-    }
-    return false;
-  }
-
   /**
    * Gets if the driver station attached to a Field Management System.
    *
    * @return true if the robot is competing on a field being controlled by a Field Management System
    */
   public static boolean isFMSAttached() {
-    m_controlWordMutex.lock();
+    m_cacheDataMutex.lock();
     try {
-      updateControlWord(false);
-      return m_controlWordCache.getFMSAttached();
+      return m_controlWord.getFMSAttached();
     } finally {
-      m_controlWordMutex.unlock();
+      m_cacheDataMutex.unlock();
     }
   }
 
@@ -1121,7 +1056,7 @@ public final class DriverStation {
    * @return the current alliance
    */
   public static Alliance getAlliance() {
-    AllianceStationID allianceStationID = HAL.getAllianceStation();
+    AllianceStationID allianceStationID = DriverStationJNI.getAllianceStation();
     if (allianceStationID == null) {
       return Alliance.Invalid;
     }
@@ -1148,7 +1083,7 @@ public final class DriverStation {
    * @return the location of the team's driver station controls: 1, 2, or 3
    */
   public static int getLocation() {
-    AllianceStationID allianceStationID = HAL.getAllianceStation();
+    AllianceStationID allianceStationID = DriverStationJNI.getAllianceStation();
     if (allianceStationID == null) {
       return 0;
     }
@@ -1170,68 +1105,6 @@ public final class DriverStation {
     }
   }
 
-  /**
-   * Wait for new data from the driver station.
-   *
-   * <p>Checks if new control data has arrived since the last waitForData call on the current
-   * thread. If new data has not arrived, returns immediately.
-   */
-  public static void waitForData() {
-    waitForData(0);
-  }
-
-  /**
-   * Wait for new data or for timeout, which ever comes first. If timeout is 0, wait for new data
-   * only.
-   *
-   * <p>Checks if new control data has arrived since the last waitForData call on the current
-   * thread. If new data has not arrived, returns immediately.
-   *
-   * @param timeoutSeconds The maximum time in seconds to wait.
-   * @return true if there is new data, otherwise false
-   */
-  public static boolean waitForData(double timeoutSeconds) {
-    long startTimeMicroS = RobotController.getFPGATime();
-    long timeoutMicroS = (long) (timeoutSeconds * 1e6);
-    m_waitForDataMutex.lock();
-    try {
-      int currentCount = m_waitForDataCount;
-      if (m_lastCount.get() != currentCount) {
-        m_lastCount.set(currentCount);
-        return true;
-      }
-      while (m_waitForDataCount == currentCount) {
-        if (timeoutMicroS > 0) {
-          long nowMicroS = RobotController.getFPGATime();
-          if (nowMicroS < startTimeMicroS + timeoutMicroS) {
-            // We still have time to wait
-            boolean signaled =
-                m_waitForDataCond.await(
-                    startTimeMicroS + timeoutMicroS - nowMicroS, TimeUnit.MICROSECONDS);
-            if (!signaled) {
-              // Return false if a timeout happened
-              return false;
-            }
-          } else {
-            // Time has elapsed.
-            return false;
-          }
-        } else {
-          m_waitForDataCond.await();
-        }
-      }
-      m_lastCount.set(m_waitForDataCount);
-      // Return true if we have received a proper signal
-      return true;
-    } catch (InterruptedException ex) {
-      // return false on a thread interrupt
-      Thread.currentThread().interrupt();
-      return false;
-    } finally {
-      m_waitForDataMutex.unlock();
-    }
-  }
-
   /**
    * Return the approximate match time. The FMS does not send an official match time to the robots,
    * but does send an approximate match time. The value will count down the time remaining in the
@@ -1242,58 +1115,7 @@ public final class DriverStation {
    * @return Time remaining in current match period (auto or teleop) in seconds
    */
   public static double getMatchTime() {
-    return HAL.getMatchTime();
-  }
-
-  /**
-   * Only to be used to tell the Driver Station what code you claim to be executing for diagnostic
-   * purposes only.
-   *
-   * @param entering If true, starting disabled code; if false, leaving disabled code
-   */
-  public static void inDisabled(boolean entering) {
-    m_userInDisabled = entering;
-  }
-
-  /**
-   * Only to be used to tell the Driver Station what code you claim to be executing for diagnostic
-   * purposes only.
-   *
-   * @param entering If true, starting autonomous code; if false, leaving autonomous code
-   */
-  public static void inAutonomous(boolean entering) {
-    m_userInAutonomous = entering;
-  }
-
-  /**
-   * Only to be used to tell the Driver Station what code you claim to be executing for diagnostic
-   * purposes only.
-   *
-   * @param entering If true, starting teleop code; if false, leaving teleop code
-   */
-  public static void inTeleop(boolean entering) {
-    m_userInTeleop = entering;
-  }
-
-  /**
-   * Only to be used to tell the Driver Station what code you claim to be executing for diagnostic
-   * purposes only.
-   *
-   * @param entering If true, starting test code; if false, leaving test code
-   */
-  public static void inTest(boolean entering) {
-    m_userInTest = entering;
-  }
-
-  /** Forces waitForData() to return immediately. */
-  public static void wakeupWaitForData() {
-    m_waitForDataMutex.lock();
-    try {
-      m_waitForDataCount++;
-      m_waitForDataCond.signalAll();
-    } finally {
-      m_waitForDataMutex.unlock();
-    }
+    return DriverStationJNI.getMatchTime();
   }
 
   /**
@@ -1317,30 +1139,43 @@ public final class DriverStation {
     return !isFMSAttached() && m_silenceJoystickWarning;
   }
 
+  /**
+   * Refresh the passed in control word to contain the current control word cache.
+   *
+   * @param word Word to refresh.
+   */
+  public static void refreshControlWordFromCache(ControlWord word) {
+    m_cacheDataMutex.lock();
+    try {
+      word.update(m_controlWord);
+    } finally {
+      m_cacheDataMutex.unlock();
+    }
+  }
+
   /**
    * Copy data from the DS task for the user. If no new data exists, it will just be returned,
    * otherwise the data will be copied from the DS polling loop.
    */
-  protected static void getData() {
+  public static void refreshData() {
+    DriverStationJNI.refreshDSData();
+
     // Get the status of all of the joysticks
     for (byte stick = 0; stick < kJoystickPorts; stick++) {
       m_joystickAxesCache[stick].m_count =
-          HAL.getJoystickAxes(stick, m_joystickAxesCache[stick].m_axes);
+          DriverStationJNI.getJoystickAxes(stick, m_joystickAxesCache[stick].m_axes);
+      m_joystickAxesRawCache[stick].m_count =
+          DriverStationJNI.getJoystickAxesRaw(stick, m_joystickAxesRawCache[stick].m_axes);
       m_joystickPOVsCache[stick].m_count =
-          HAL.getJoystickPOVs(stick, m_joystickPOVsCache[stick].m_povs);
-      m_joystickButtonsCache[stick].m_buttons = HAL.getJoystickButtons(stick, m_buttonCountBuffer);
+          DriverStationJNI.getJoystickPOVs(stick, m_joystickPOVsCache[stick].m_povs);
+      m_joystickButtonsCache[stick].m_buttons =
+          DriverStationJNI.getJoystickButtons(stick, m_buttonCountBuffer);
       m_joystickButtonsCache[stick].m_count = m_buttonCountBuffer.get(0);
     }
 
-    HAL.getMatchInfo(m_matchInfoCache);
+    DriverStationJNI.getMatchInfo(m_matchInfoCache);
 
-    m_controlWordMutex.lock();
-    try {
-      // Force a control word update, to make sure the data is the newest.
-      updateControlWord(true);
-    } finally {
-      m_controlWordMutex.unlock();
-    }
+    DriverStationJNI.getControlWord(m_controlWordCache);
 
     DataLogSender dataLogSender;
     // lock joystick mutex to swap cache data
@@ -1361,6 +1196,10 @@ public final class DriverStation {
       m_joystickAxes = m_joystickAxesCache;
       m_joystickAxesCache = currentAxes;
 
+      HALJoystickAxesRaw[] currentAxesRaw = m_joystickAxesRaw;
+      m_joystickAxesRaw = m_joystickAxesRawCache;
+      m_joystickAxesRawCache = currentAxesRaw;
+
       HALJoystickButtons[] currentButtons = m_joystickButtons;
       m_joystickButtons = m_joystickButtonsCache;
       m_joystickButtonsCache = currentButtons;
@@ -1373,18 +1212,31 @@ public final class DriverStation {
       m_matchInfo = m_matchInfoCache;
       m_matchInfoCache = currentInfo;
 
+      ControlWord currentWord = m_controlWord;
+      m_controlWord = m_controlWordCache;
+      m_controlWordCache = currentWord;
+
       dataLogSender = m_dataLogSender;
     } finally {
       m_cacheDataMutex.unlock();
     }
 
-    wakeupWaitForData();
+    m_refreshEvents.wakeup();
+
     m_matchDataSender.sendMatchData();
     if (dataLogSender != null) {
       dataLogSender.send(WPIUtilJNI.now());
     }
   }
 
+  public static void provideRefreshedDataEventHandle(int handle) {
+    m_refreshEvents.add(handle);
+  }
+
+  public static void removeRefreshedDataEventHandle(int handle) {
+    m_refreshEvents.remove(handle);
+  }
+
   /**
    * Reports errors related to unplugged joysticks Throttles the errors so that they don't overwhelm
    * the DS.
@@ -1411,68 +1263,6 @@ public final class DriverStation {
     }
   }
 
-  /** Provides the service routine for the DS polling m_thread. */
-  private static void run() {
-    int safetyCounter = 0;
-    while (m_threadKeepAlive) {
-      HAL.waitForDSData();
-      getData();
-
-      if (isDisabled()) {
-        safetyCounter = 0;
-      }
-
-      safetyCounter++;
-      if (safetyCounter >= 4) {
-        MotorSafety.checkMotors();
-        safetyCounter = 0;
-      }
-      if (m_userInDisabled) {
-        HAL.observeUserProgramDisabled();
-      }
-      if (m_userInAutonomous) {
-        HAL.observeUserProgramAutonomous();
-      }
-      if (m_userInTeleop) {
-        HAL.observeUserProgramTeleop();
-      }
-      if (m_userInTest) {
-        HAL.observeUserProgramTest();
-      }
-    }
-  }
-
-  /**
-   * Forces a control word cache update, and update the passed in control word.
-   *
-   * @param word Word to update.
-   */
-  public static void updateControlWordFromCache(ControlWord word) {
-    m_controlWordMutex.lock();
-    try {
-      updateControlWord(true);
-      word.update(m_controlWordCache);
-    } finally {
-      m_controlWordMutex.unlock();
-    }
-  }
-
-  /**
-   * Updates the data in the control word cache. Updates if the force parameter is set, or if 50ms
-   * have passed since the last update.
-   *
-   * <p>Must be called with m_controlWordMutex lock held.
-   *
-   * @param force True to force an update to the cache, otherwise update if 50ms have passed.
-   */
-  private static void updateControlWord(boolean force) {
-    long now = System.currentTimeMillis();
-    if (now - m_lastControlWordUpdate > 50 || force) {
-      HAL.getControlWord(m_controlWordCache);
-      m_lastControlWordUpdate = now;
-    }
-  }
-
   /**
    * Starts logging DriverStation data to data log. Repeated calls are ignored.
    *

wpilibj/src/main/java/edu/wpi/first/wpilibj/GenericHID.java

@@ -4,7 +4,7 @@
 
 package edu.wpi.first.wpilibj;
 
-import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.DriverStationJNI;
 import edu.wpi.first.wpilibj.event.BooleanEvent;
 import edu.wpi.first.wpilibj.event.EventLoop;
 import java.util.HashMap;
@@ -248,7 +248,7 @@ public class GenericHID {
    */
   public void setOutput(int outputNumber, boolean value) {
     m_outputs = (m_outputs & ~(1 << (outputNumber - 1))) | ((value ? 1 : 0) << (outputNumber - 1));
-    HAL.setJoystickOutputs((byte) m_port, m_outputs, m_leftRumble, m_rightRumble);
+    DriverStationJNI.setJoystickOutputs((byte) m_port, m_outputs, m_leftRumble, m_rightRumble);
   }
 
   /**
@@ -258,7 +258,7 @@ public class GenericHID {
    */
   public void setOutputs(int value) {
     m_outputs = value;
-    HAL.setJoystickOutputs((byte) m_port, m_outputs, m_leftRumble, m_rightRumble);
+    DriverStationJNI.setJoystickOutputs((byte) m_port, m_outputs, m_leftRumble, m_rightRumble);
   }
 
   /**
@@ -279,6 +279,6 @@ public class GenericHID {
     } else {
       m_rightRumble = (short) (value * 65535);
     }
-    HAL.setJoystickOutputs((byte) m_port, m_outputs, m_leftRumble, m_rightRumble);
+    DriverStationJNI.setJoystickOutputs((byte) m_port, m_outputs, m_leftRumble, m_rightRumble);
   }
 }

wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java

@@ -4,6 +4,7 @@
 
 package edu.wpi.first.wpilibj;
 
+import edu.wpi.first.hal.DriverStationJNI;
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.livewindow.LiveWindow;
@@ -253,10 +254,12 @@ public abstract class IterativeRobotBase extends RobotBase {
   }
 
   protected void loopFunc() {
+    DriverStation.refreshData();
     m_watchdog.reset();
 
+    m_word.refresh();
+
     // Get current mode
-    m_word.update();
     Mode mode = Mode.kNone;
     if (m_word.isDisabled()) {
       mode = Mode.kDisabled;
@@ -305,19 +308,19 @@ public abstract class IterativeRobotBase extends RobotBase {
 
     // Call the appropriate function depending upon the current robot mode
     if (mode == Mode.kDisabled) {
-      HAL.observeUserProgramDisabled();
+      DriverStationJNI.observeUserProgramDisabled();
       disabledPeriodic();
       m_watchdog.addEpoch("disabledPeriodic()");
     } else if (mode == Mode.kAutonomous) {
-      HAL.observeUserProgramAutonomous();
+      DriverStationJNI.observeUserProgramAutonomous();
       autonomousPeriodic();
       m_watchdog.addEpoch("autonomousPeriodic()");
     } else if (mode == Mode.kTeleop) {
-      HAL.observeUserProgramTeleop();
+      DriverStationJNI.observeUserProgramTeleop();
       teleopPeriodic();
       m_watchdog.addEpoch("teleopPeriodic()");
     } else {
-      HAL.observeUserProgramTest();
+      DriverStationJNI.observeUserProgramTest();
       testPeriodic();
       m_watchdog.addEpoch("testPeriodic()");
     }

wpilibj/src/main/java/edu/wpi/first/wpilibj/MotorSafety.java

@@ -29,6 +29,7 @@ public abstract class MotorSafety {
   public MotorSafety() {
     synchronized (m_listMutex) {
       m_instanceList.add(this);
+      // TODO Threads
     }
   }
 

wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java

@@ -274,15 +274,6 @@ public abstract class RobotBase implements AutoCloseable {
     return DriverStation.isTeleopEnabled();
   }
 
-  /**
-   * Indicates if new data is available from the driver station.
-   *
-   * @return Has new data arrived over the network since the last time this function was called?
-   */
-  public boolean isNewDataAvailable() {
-    return DriverStation.isNewControlData();
-  }
-
   /** Provide an alternate "main loop" via startCompetition(). */
   public abstract void startCompetition();
 

wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java

@@ -4,6 +4,7 @@
 
 package edu.wpi.first.wpilibj;
 
+import edu.wpi.first.hal.DriverStationJNI;
 import edu.wpi.first.hal.FRCNetComm.tInstances;
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
@@ -110,7 +111,7 @@ public class TimedRobot extends IterativeRobotBase {
 
     // Tell the DS that the robot is ready to be enabled
     System.out.println("********** Robot program startup complete **********");
-    HAL.observeUserProgramStarting();
+    DriverStationJNI.observeUserProgramStarting();
 
     // Loop forever, calling the appropriate mode-dependent function
     while (true) {

wpilibj/src/main/java/edu/wpi/first/wpilibj/internal/DriverStationModeThread.java

@@ -0,0 +1,112 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.internal;
+
+import edu.wpi.first.hal.DriverStationJNI;
+import edu.wpi.first.util.WPIUtilJNI;
+import edu.wpi.first.wpilibj.DriverStation;
+import java.util.concurrent.atomic.AtomicBoolean;
+
+/*
+ * For internal use only.
+ */
+public class DriverStationModeThread implements AutoCloseable {
+  private final AtomicBoolean m_keepAlive = new AtomicBoolean();
+  private final Thread m_thread;
+
+  private boolean m_userInDisabled;
+  private boolean m_userInAutonomous;
+  private boolean m_userInTeleop;
+  private boolean m_userInTest;
+
+  /** Internal use only. */
+  public DriverStationModeThread() {
+    m_keepAlive.set(true);
+    m_thread = new Thread(this::run, "DriverStationMode");
+    m_thread.start();
+  }
+
+  private void run() {
+    int handle = WPIUtilJNI.createEvent(true, false);
+    DriverStationJNI.provideNewDataEventHandle(handle);
+
+    while (m_keepAlive.get()) {
+      try {
+        WPIUtilJNI.waitForObjectTimeout(handle, 0.1);
+      } catch (InterruptedException e) {
+        DriverStationJNI.removeNewDataEventHandle(handle);
+        WPIUtilJNI.destroyEvent(handle);
+        Thread.currentThread().interrupt();
+        return;
+      }
+      DriverStation.refreshData();
+      if (m_userInDisabled) {
+        DriverStationJNI.observeUserProgramDisabled();
+      }
+      if (m_userInAutonomous) {
+        DriverStationJNI.observeUserProgramAutonomous();
+      }
+      if (m_userInTeleop) {
+        DriverStationJNI.observeUserProgramTeleop();
+      }
+      if (m_userInTest) {
+        DriverStationJNI.observeUserProgramTest();
+      }
+    }
+
+    DriverStationJNI.removeNewDataEventHandle(handle);
+    WPIUtilJNI.destroyEvent(handle);
+  }
+
+  /**
+   * Only to be used to tell the Driver Station what code you claim to be executing for diagnostic
+   * purposes only.
+   *
+   * @param entering If true, starting disabled code; if false, leaving disabled code
+   */
+  public void inDisabled(boolean entering) {
+    m_userInDisabled = entering;
+  }
+
+  /**
+   * Only to be used to tell the Driver Station what code you claim to be executing for diagnostic
+   * purposes only.
+   *
+   * @param entering If true, starting autonomous code; if false, leaving autonomous code
+   */
+  public void inAutonomous(boolean entering) {
+    m_userInAutonomous = entering;
+  }
+
+  /**
+   * Only to be used to tell the Driver Station what code you claim to be executing for diagnostic
+   * purposes only.
+   *
+   * @param entering If true, starting teleop code; if false, leaving teleop code
+   */
+  public void inTeleop(boolean entering) {
+    m_userInTeleop = entering;
+  }
+
+  /**
+   * Only to be used to tell the Driver Station what code you claim to be executing for diagnostic
+   * purposes only.
+   *
+   * @param entering If true, starting test code; if false, leaving test code
+   */
+  public void inTest(boolean entering) {
+    m_userInTest = entering;
+  }
+
+  @Override
+  public void close() {
+    m_keepAlive.set(false);
+    try {
+      m_thread.join();
+    } catch (InterruptedException e) {
+      Thread.currentThread().interrupt();
+    }
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DriverStationSim.java

@@ -5,8 +5,10 @@
 package edu.wpi.first.wpilibj.simulation;
 
 import edu.wpi.first.hal.AllianceStationID;
+import edu.wpi.first.hal.DriverStationJNI;
 import edu.wpi.first.hal.simulation.DriverStationDataJNI;
 import edu.wpi.first.hal.simulation.NotifyCallback;
+import edu.wpi.first.util.WPIUtilJNI;
 import edu.wpi.first.wpilibj.DriverStation;
 
 /** Class to control a simulated driver station. */
@@ -310,8 +312,17 @@ public final class DriverStationSim {
 
   /** Updates DriverStation data so that new values are visible to the user program. */
   public static void notifyNewData() {
+    int handle = WPIUtilJNI.createEvent(false, false);
+    DriverStationJNI.provideNewDataEventHandle(handle);
     DriverStationDataJNI.notifyNewData();
-    DriverStation.waitForData();
+    try {
+      WPIUtilJNI.waitForObject(handle);
+    } catch (InterruptedException e) {
+      e.printStackTrace();
+    }
+    DriverStationJNI.removeNewDataEventHandle(handle);
+    WPIUtilJNI.destroyEvent(handle);
+    DriverStation.refreshData();
   }
 
   /**