Revert "MotorSafety: Use Watchdog instead of DS class polling (#1442)"

This reverts commit 26e8e587f9.

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

@@ -1116,10 +1116,20 @@ public class DriverStation {
    * Provides the service routine for the DS polling m_thread.
    */
   private 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();
       }

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

@@ -7,6 +7,9 @@
 
 package edu.wpi.first.wpilibj;
 
+import java.util.LinkedHashSet;
+import java.util.Set;
+
 /**
  * This base class runs a watchdog timer and calls the subclass's StopMotor()
  * function if the timeout expires.
@@ -16,9 +19,21 @@ package edu.wpi.first.wpilibj;
 public abstract class MotorSafety {
   private static final double kDefaultSafetyExpiration = 0.1;
 
-  private final Watchdog m_watchdog = new Watchdog(kDefaultSafetyExpiration, this::timeoutFunc);
+  private double m_expiration = kDefaultSafetyExpiration;
   private boolean m_enabled;
+  private double m_stopTime = Timer.getFPGATimestamp();
   private final Object m_thisMutex = new Object();
+  private static final Set<MotorSafety> m_instanceList = new LinkedHashSet<>();
+  private static final Object m_listMutex = new Object();
+
+  /**
+   * MotorSafety constructor.
+   */
+  public MotorSafety() {
+    synchronized (m_listMutex) {
+      m_instanceList.add(this);
+    }
+  }
 
   /**
    * Feed the motor safety object.
@@ -27,7 +42,7 @@ public abstract class MotorSafety {
    */
   public void feed() {
     synchronized (m_thisMutex) {
-      m_watchdog.reset();
+      m_stopTime = Timer.getFPGATimestamp() + m_expiration;
     }
   }
 
@@ -38,7 +53,7 @@ public abstract class MotorSafety {
    */
   public void setExpiration(double expirationTime) {
     synchronized (m_thisMutex) {
-      m_watchdog.setTimeout(expirationTime);
+      m_expiration = expirationTime;
     }
   }
 
@@ -49,7 +64,7 @@ public abstract class MotorSafety {
    */
   public double getExpiration() {
     synchronized (m_thisMutex) {
-      return m_watchdog.getTimeout();
+      return m_expiration;
     }
   }
 
@@ -60,7 +75,32 @@ public abstract class MotorSafety {
    */
   public boolean isAlive() {
     synchronized (m_thisMutex) {
-      return !m_enabled || !m_watchdog.isExpired();
+      return !m_enabled || m_stopTime > Timer.getFPGATimestamp();
+    }
+  }
+
+  /**
+   * Check if this motor has exceeded its timeout. This method is called periodically to determine
+   * if this motor has exceeded its timeout value. If it has, the stop method is called, and the
+   * motor is shut down until its value is updated again.
+   */
+  public void check() {
+    boolean enabled;
+    double stopTime;
+
+    synchronized (m_thisMutex) {
+      enabled = m_enabled;
+      stopTime = m_stopTime;
+    }
+
+    if (!enabled || RobotState.isDisabled() || RobotState.isTest()) {
+      return;
+    }
+
+    if (stopTime < Timer.getFPGATimestamp()) {
+      DriverStation.reportError(getDescription() + "... Output not updated often enough.", false);
+
+      stopMotor();
     }
   }
 
@@ -73,11 +113,6 @@ public abstract class MotorSafety {
    */
   public void setSafetyEnabled(boolean enabled) {
     synchronized (m_thisMutex) {
-      if (enabled) {
-        m_watchdog.enable();
-      } else {
-        m_watchdog.disable();
-      }
       m_enabled = enabled;
     }
   }
@@ -95,15 +130,18 @@ public abstract class MotorSafety {
     }
   }
 
-  private void timeoutFunc() {
-    DriverStation ds = DriverStation.getInstance();
-    if (ds.isDisabled() || ds.isTest()) {
-      return;
+  /**
+   * Check the motors to see if any have timed out.
+   *
+   * <p>This static method is called periodically to poll all the motors and stop any that have
+   * timed out.
+   */
+  public static void checkMotors() {
+    synchronized (m_listMutex) {
+      for (MotorSafety elem : m_instanceList) {
+        elem.check();
+      }
     }
-
-    DriverStation.reportError(getDescription() + "... Output not updated often enough.", false);
-
-    stopMotor();
   }
 
   public abstract void stopMotor();

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

@@ -76,7 +76,7 @@ public class Watchdog implements Closeable, Comparable<Watchdog> {
   }
 
   /**
-   * Returns the time in seconds since the watchdog was last fed.
+   * Get the time in seconds since the watchdog was last fed.
    */
   public double getTime() {
     return (RobotController.getFPGATime() - m_startTime) / 1.0e6;
@@ -187,7 +187,7 @@ public class Watchdog implements Closeable, Comparable<Watchdog> {
   }
 
   /**
-   * Disables the watchdog timer.
+   * Disable the watchdog.
    */
   public void disable() {
     m_queueMutex.lock();