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

wpilibc/src/main/native/cpp/DriverStation.cpp

@@ -19,7 +19,6 @@
 #include <wpi/StringRef.h>
 
 #include "frc/AnalogInput.h"
-#include "frc/MotorSafety.h"
 #include "frc/Timer.h"
 #include "frc/Utility.h"
 #include "frc/WPIErrors.h"
@@ -549,17 +548,10 @@ void DriverStation::ReportJoystickUnpluggedWarning(const wpi::Twine& message) {
 
 void DriverStation::Run() {
   m_isRunning = true;
-  int safetyCounter = 0;
   while (m_isRunning) {
     HAL_WaitForDSData();
     GetData();
 
-    if (IsDisabled()) safetyCounter = 0;
-
-    if (++safetyCounter >= 4) {
-      MotorSafety::CheckMotors();
-      safetyCounter = 0;
-    }
     if (m_userInDisabled) HAL_ObserveUserProgramDisabled();
     if (m_userInAutonomous) HAL_ObserveUserProgramAutonomous();
     if (m_userInTeleop) HAL_ObserveUserProgramTeleop();

wpilibc/src/main/native/cpp/MotorSafety.cpp

@@ -10,66 +10,53 @@
 #include <algorithm>
 #include <utility>
 
-#include <wpi/SmallPtrSet.h>
 #include <wpi/SmallString.h>
-#include <wpi/raw_ostream.h>
 
-#include "frc/DriverStation.h"
 #include "frc/WPIErrors.h"
 
 using namespace frc;
 
-static wpi::SmallPtrSet<MotorSafety*, 32> instanceList;
-static wpi::mutex listMutex;
-
-MotorSafety::MotorSafety() {
-  std::lock_guard<wpi::mutex> lock(listMutex);
-  instanceList.insert(this);
-}
-
-MotorSafety::~MotorSafety() {
-  std::lock_guard<wpi::mutex> lock(listMutex);
-  instanceList.erase(this);
-}
-
 MotorSafety::MotorSafety(MotorSafety&& rhs)
-    : ErrorBase(std::move(rhs)),
-      m_expiration(std::move(rhs.m_expiration)),
-      m_enabled(std::move(rhs.m_enabled)),
-      m_stopTime(std::move(rhs.m_stopTime)) {}
+    : ErrorBase(std::move(rhs)), m_enabled(std::move(rhs.m_enabled)) {
+  m_watchdog = Watchdog(rhs.m_watchdog.GetTimeout(), [this] { TimeoutFunc(); });
+}
 
 MotorSafety& MotorSafety::operator=(MotorSafety&& rhs) {
   ErrorBase::operator=(std::move(rhs));
 
-  m_expiration = std::move(rhs.m_expiration);
+  m_watchdog = Watchdog(rhs.m_watchdog.GetTimeout(), [this] { TimeoutFunc(); });
   m_enabled = std::move(rhs.m_enabled);
-  m_stopTime = std::move(rhs.m_stopTime);
 
   return *this;
 }
 
 void MotorSafety::Feed() {
   std::lock_guard<wpi::mutex> lock(m_thisMutex);
-  m_stopTime = Timer::GetFPGATimestamp() + m_expiration;
+  m_watchdog.Reset();
 }
 
 void MotorSafety::SetExpiration(double expirationTime) {
   std::lock_guard<wpi::mutex> lock(m_thisMutex);
-  m_expiration = expirationTime;
+  m_watchdog.SetTimeout(expirationTime);
 }
 
 double MotorSafety::GetExpiration() const {
   std::lock_guard<wpi::mutex> lock(m_thisMutex);
-  return m_expiration;
+  return m_watchdog.GetTimeout();
 }
 
 bool MotorSafety::IsAlive() const {
   std::lock_guard<wpi::mutex> lock(m_thisMutex);
-  return !m_enabled || m_stopTime > Timer::GetFPGATimestamp();
+  return !m_enabled || !m_watchdog.IsExpired();
 }
 
 void MotorSafety::SetSafetyEnabled(bool enabled) {
   std::lock_guard<wpi::mutex> lock(m_thisMutex);
+  if (enabled) {
+    m_watchdog.Enable();
+  } else {
+    m_watchdog.Disable();
+  }
   m_enabled = enabled;
 }
 
@@ -78,35 +65,16 @@ bool MotorSafety::IsSafetyEnabled() const {
   return m_enabled;
 }
 
-void MotorSafety::Check() {
-  bool enabled;
-  double stopTime;
-
-  {
-    std::lock_guard<wpi::mutex> lock(m_thisMutex);
-    enabled = m_enabled;
-    stopTime = m_stopTime;
-  }
-
-  DriverStation& ds = DriverStation::GetInstance();
-  if (!enabled || ds.IsDisabled() || ds.IsTest()) {
+void MotorSafety::TimeoutFunc() {
+  auto& ds = DriverStation::GetInstance();
+  if (ds.IsDisabled() || ds.IsTest()) {
     return;
   }
 
-  std::lock_guard<wpi::mutex> lock(m_thisMutex);
-  if (stopTime < Timer::GetFPGATimestamp()) {
-    wpi::SmallString<128> buf;
-    wpi::raw_svector_ostream desc(buf);
-    GetDescription(desc);
-    desc << "... Output not updated often enough.";
-    wpi_setWPIErrorWithContext(Timeout, desc.str());
-    StopMotor();
-  }
-}
-
-void MotorSafety::CheckMotors() {
-  std::lock_guard<wpi::mutex> lock(listMutex);
-  for (auto elem : instanceList) {
-    elem->Check();
-  }
+  wpi::SmallString<128> buf;
+  wpi::raw_svector_ostream desc(buf);
+  GetDescription(desc);
+  desc << "... Output not updated often enough.";
+  wpi_setWPIErrorWithContext(Timeout, desc.str());
+  StopMotor();
 }

wpilibc/src/main/native/include/frc/MotorSafety.h

@@ -10,8 +10,10 @@
 #include <wpi/mutex.h>
 #include <wpi/raw_ostream.h>
 
+#include "frc/DriverStation.h"
 #include "frc/ErrorBase.h"
 #include "frc/Timer.h"
+#include "frc/Watchdog.h"
 
 namespace frc {
 
@@ -23,8 +25,8 @@ namespace frc {
  */
 class MotorSafety : public ErrorBase {
  public:
-  MotorSafety();
-  virtual ~MotorSafety();
+  MotorSafety() = default;
+  virtual ~MotorSafety() = default;
 
   MotorSafety(MotorSafety&& rhs);
   MotorSafety& operator=(MotorSafety&& rhs);
@@ -75,39 +77,20 @@ class MotorSafety : public ErrorBase {
    */
   bool IsSafetyEnabled() const;
 
-  /**
-   * 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.
-   */
-  void Check();
-
-  /**
-   * Check the motors to see if any have timed out.
-   *
-   * This static method is called periodically to poll all the motors and stop
-   * any that have timed out.
-   */
-  static void CheckMotors();
-
   virtual void StopMotor() = 0;
   virtual void GetDescription(wpi::raw_ostream& desc) const = 0;
 
  private:
   static constexpr double kDefaultSafetyExpiration = 0.1;
 
-  // The expiration time for this object
-  double m_expiration = kDefaultSafetyExpiration;
+  Watchdog m_watchdog{kDefaultSafetyExpiration, [this] { TimeoutFunc(); }};
 
   // True if motor safety is enabled for this motor
   bool m_enabled = false;
 
-  // The FPGA clock value when the motor has expired
-  double m_stopTime = Timer::GetFPGATimestamp();
-
   mutable wpi::mutex m_thisMutex;
+
+  void TimeoutFunc();
 };
 
 }  // namespace frc

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

@@ -1116,20 +1116,10 @@ private void reportJoystickUnpluggedWarning(String message) {
    * 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,9 +7,6 @@
 
 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.
@@ -19,21 +16,9 @@
 public abstract class MotorSafety {
   private static final double kDefaultSafetyExpiration = 0.1;
 
-  private double m_expiration = kDefaultSafetyExpiration;
+  private final Watchdog m_watchdog = new Watchdog(kDefaultSafetyExpiration, this::timeoutFunc);
   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.
@@ -42,7 +27,7 @@ public MotorSafety() {
    */
   public void feed() {
     synchronized (m_thisMutex) {
-      m_stopTime = Timer.getFPGATimestamp() + m_expiration;
+      m_watchdog.reset();
     }
   }
 
@@ -53,7 +38,7 @@ public void feed() {
    */
   public void setExpiration(double expirationTime) {
     synchronized (m_thisMutex) {
-      m_expiration = expirationTime;
+      m_watchdog.setTimeout(expirationTime);
     }
   }
 
@@ -64,7 +49,7 @@ public void setExpiration(double expirationTime) {
    */
   public double getExpiration() {
     synchronized (m_thisMutex) {
-      return m_expiration;
+      return m_watchdog.getTimeout();
     }
   }
 
@@ -75,32 +60,7 @@ public double getExpiration() {
    */
   public boolean isAlive() {
     synchronized (m_thisMutex) {
-      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();
+      return !m_enabled || !m_watchdog.isExpired();
     }
   }
 
@@ -113,6 +73,11 @@ public void check() {
    */
   public void setSafetyEnabled(boolean enabled) {
     synchronized (m_thisMutex) {
+      if (enabled) {
+        m_watchdog.enable();
+      } else {
+        m_watchdog.disable();
+      }
       m_enabled = enabled;
     }
   }
@@ -130,18 +95,15 @@ public boolean isSafetyEnabled() {
     }
   }
 
-  /**
-   * 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();
-      }
+  private void timeoutFunc() {
+    DriverStation ds = DriverStation.getInstance();
+    if (ds.isDisabled() || ds.isTest()) {
+      return;
     }
+
+    DriverStation.reportError(getDescription() + "... Output not updated often enough.", false);
+
+    stopMotor();
   }
 
   public abstract void stopMotor();

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

@@ -71,7 +71,7 @@ public int compareTo(Watchdog rhs) {
   }
 
   /**
-   * Get the time in seconds since the watchdog was last fed.
+   * Returns the time in seconds since the watchdog was last fed.
    */
   public double getTime() {
     return (RobotController.getFPGATime() - m_startTime) / 1.0e6;
@@ -178,7 +178,7 @@ public void enable() {
   }
 
   /**
-   * Disable the watchdog.
+   * Disables the watchdog timer.
    */
   public void disable() {
     m_queueMutex.lock();