diff --git a/wpilibc/src/main/native/cpp/DriverStation.cpp b/wpilibc/src/main/native/cpp/DriverStation.cpp index fe657211b6..7b379b3b58 100644 --- a/wpilibc/src/main/native/cpp/DriverStation.cpp +++ b/wpilibc/src/main/native/cpp/DriverStation.cpp @@ -19,7 +19,6 @@ #include #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(); diff --git a/wpilibc/src/main/native/cpp/MotorSafety.cpp b/wpilibc/src/main/native/cpp/MotorSafety.cpp index 88ccee76c0..56c846fcd4 100644 --- a/wpilibc/src/main/native/cpp/MotorSafety.cpp +++ b/wpilibc/src/main/native/cpp/MotorSafety.cpp @@ -10,66 +10,53 @@ #include #include -#include #include -#include -#include "frc/DriverStation.h" #include "frc/WPIErrors.h" using namespace frc; -static wpi::SmallPtrSet instanceList; -static wpi::mutex listMutex; - -MotorSafety::MotorSafety() { - std::lock_guard lock(listMutex); - instanceList.insert(this); -} - -MotorSafety::~MotorSafety() { - std::lock_guard 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 lock(m_thisMutex); - m_stopTime = Timer::GetFPGATimestamp() + m_expiration; + m_watchdog.Reset(); } void MotorSafety::SetExpiration(double expirationTime) { std::lock_guard lock(m_thisMutex); - m_expiration = expirationTime; + m_watchdog.SetTimeout(expirationTime); } double MotorSafety::GetExpiration() const { std::lock_guard lock(m_thisMutex); - return m_expiration; + return m_watchdog.GetTimeout(); } bool MotorSafety::IsAlive() const { std::lock_guard lock(m_thisMutex); - return !m_enabled || m_stopTime > Timer::GetFPGATimestamp(); + return !m_enabled || !m_watchdog.IsExpired(); } void MotorSafety::SetSafetyEnabled(bool enabled) { std::lock_guard 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 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 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 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(); } diff --git a/wpilibc/src/main/native/include/frc/MotorSafety.h b/wpilibc/src/main/native/include/frc/MotorSafety.h index afd0853ef3..513af392f5 100644 --- a/wpilibc/src/main/native/include/frc/MotorSafety.h +++ b/wpilibc/src/main/native/include/frc/MotorSafety.h @@ -10,8 +10,10 @@ #include #include +#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 diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java index 913b51969f..8dc44f15f1 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java @@ -1116,20 +1116,10 @@ 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(); } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/MotorSafety.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/MotorSafety.java index 5dd3a4f13a..be455e6bb1 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/MotorSafety.java +++ b/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 @@ import java.util.Set; 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 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 abstract class MotorSafety { */ public void feed() { synchronized (m_thisMutex) { - m_stopTime = Timer.getFPGATimestamp() + m_expiration; + m_watchdog.reset(); } } @@ -53,7 +38,7 @@ public abstract class MotorSafety { */ public void setExpiration(double expirationTime) { synchronized (m_thisMutex) { - m_expiration = expirationTime; + m_watchdog.setTimeout(expirationTime); } } @@ -64,7 +49,7 @@ public abstract class MotorSafety { */ public double getExpiration() { synchronized (m_thisMutex) { - return m_expiration; + return m_watchdog.getTimeout(); } } @@ -75,32 +60,7 @@ public abstract class MotorSafety { */ 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 abstract class MotorSafety { */ 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 abstract class MotorSafety { } } - /** - * 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. - */ - 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(); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java index 998836e479..180a380fef 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java @@ -71,7 +71,7 @@ public class Watchdog implements Closeable, Comparable { } /** - * 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 class Watchdog implements Closeable, Comparable { } /** - * Disable the watchdog. + * Disables the watchdog timer. */ public void disable() { m_queueMutex.lock();