diff --git a/wpilibc/src/main/native/cpp/DriverStation.cpp b/wpilibc/src/main/native/cpp/DriverStation.cpp index 96247041a7..3c274c6ef0 100644 --- a/wpilibc/src/main/native/cpp/DriverStation.cpp +++ b/wpilibc/src/main/native/cpp/DriverStation.cpp @@ -19,6 +19,7 @@ #include #include "frc/AnalogInput.h" +#include "frc/MotorSafety.h" #include "frc/Timer.h" #include "frc/Utility.h" #include "frc/WPIErrors.h" @@ -548,10 +549,17 @@ 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 56c846fcd4..88ccee76c0 100644 --- a/wpilibc/src/main/native/cpp/MotorSafety.cpp +++ b/wpilibc/src/main/native/cpp/MotorSafety.cpp @@ -10,53 +10,66 @@ #include #include +#include #include +#include +#include "frc/DriverStation.h" #include "frc/WPIErrors.h" using namespace frc; -MotorSafety::MotorSafety(MotorSafety&& rhs) - : ErrorBase(std::move(rhs)), m_enabled(std::move(rhs.m_enabled)) { - m_watchdog = Watchdog(rhs.m_watchdog.GetTimeout(), [this] { TimeoutFunc(); }); +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)) {} + MotorSafety& MotorSafety::operator=(MotorSafety&& rhs) { ErrorBase::operator=(std::move(rhs)); - m_watchdog = Watchdog(rhs.m_watchdog.GetTimeout(), [this] { TimeoutFunc(); }); + m_expiration = std::move(rhs.m_expiration); 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_watchdog.Reset(); + m_stopTime = Timer::GetFPGATimestamp() + m_expiration; } void MotorSafety::SetExpiration(double expirationTime) { std::lock_guard lock(m_thisMutex); - m_watchdog.SetTimeout(expirationTime); + m_expiration = expirationTime; } double MotorSafety::GetExpiration() const { std::lock_guard lock(m_thisMutex); - return m_watchdog.GetTimeout(); + return m_expiration; } bool MotorSafety::IsAlive() const { std::lock_guard lock(m_thisMutex); - return !m_enabled || !m_watchdog.IsExpired(); + return !m_enabled || m_stopTime > Timer::GetFPGATimestamp(); } void MotorSafety::SetSafetyEnabled(bool enabled) { std::lock_guard lock(m_thisMutex); - if (enabled) { - m_watchdog.Enable(); - } else { - m_watchdog.Disable(); - } m_enabled = enabled; } @@ -65,16 +78,35 @@ bool MotorSafety::IsSafetyEnabled() const { return m_enabled; } -void MotorSafety::TimeoutFunc() { - auto& ds = DriverStation::GetInstance(); - if (ds.IsDisabled() || ds.IsTest()) { +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()) { return; } - wpi::SmallString<128> buf; - wpi::raw_svector_ostream desc(buf); - GetDescription(desc); - desc << "... Output not updated often enough."; - wpi_setWPIErrorWithContext(Timeout, desc.str()); - StopMotor(); + 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(); + } } diff --git a/wpilibc/src/main/native/include/frc/MotorSafety.h b/wpilibc/src/main/native/include/frc/MotorSafety.h index 513af392f5..afd0853ef3 100644 --- a/wpilibc/src/main/native/include/frc/MotorSafety.h +++ b/wpilibc/src/main/native/include/frc/MotorSafety.h @@ -10,10 +10,8 @@ #include #include -#include "frc/DriverStation.h" #include "frc/ErrorBase.h" #include "frc/Timer.h" -#include "frc/Watchdog.h" namespace frc { @@ -25,8 +23,8 @@ namespace frc { */ class MotorSafety : public ErrorBase { public: - MotorSafety() = default; - virtual ~MotorSafety() = default; + MotorSafety(); + virtual ~MotorSafety(); MotorSafety(MotorSafety&& rhs); MotorSafety& operator=(MotorSafety&& rhs); @@ -77,20 +75,39 @@ 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; - Watchdog m_watchdog{kDefaultSafetyExpiration, [this] { TimeoutFunc(); }}; + // The expiration time for this object + double m_expiration = kDefaultSafetyExpiration; // True if motor safety is enabled for this motor bool m_enabled = false; - mutable wpi::mutex m_thisMutex; + // The FPGA clock value when the motor has expired + double m_stopTime = Timer::GetFPGATimestamp(); - void TimeoutFunc(); + mutable wpi::mutex m_thisMutex; }; } // 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 d5f3dfb078..b5c46c9a96 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java +++ b/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(); } 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 be455e6bb1..5dd3a4f13a 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/MotorSafety.java +++ b/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 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. + * + *

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(); 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 1af081470e..0b99c45ee3 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java @@ -76,7 +76,7 @@ public class Watchdog implements Closeable, Comparable { } /** - * 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 { } /** - * Disables the watchdog timer. + * Disable the watchdog. */ public void disable() { m_queueMutex.lock();