diff --git a/wpilibc/src/main/native/cpp/IterativeRobot.cpp b/wpilibc/src/main/native/cpp/IterativeRobot.cpp index 7809d66f0d..47380038ca 100644 --- a/wpilibc/src/main/native/cpp/IterativeRobot.cpp +++ b/wpilibc/src/main/native/cpp/IterativeRobot.cpp @@ -13,7 +13,9 @@ using namespace frc; -IterativeRobot::IterativeRobot() { +static constexpr double kPacketPeriod = 0.02; + +IterativeRobot::IterativeRobot() : IterativeRobotBase(kPacketPeriod) { HAL_Report(HALUsageReporting::kResourceType_Framework, HALUsageReporting::kFramework_Iterative); } diff --git a/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp b/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp index a122719586..8bdaea25ed 100644 --- a/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp +++ b/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp @@ -10,14 +10,21 @@ #include #include +#include #include #include "Commands/Scheduler.h" +#include "DriverStation.h" #include "LiveWindow/LiveWindow.h" #include "SmartDashboard/SmartDashboard.h" +#include "Timer.h" using namespace frc; +IterativeRobotBase::IterativeRobotBase(double period) + : m_period(period), + m_watchdog(period, [&] { PrintLoopOverrunMessage(); }) {} + void IterativeRobotBase::RobotInit() { wpi::outs() << "Default " << __FUNCTION__ << "() method... Overload me!\n"; } @@ -79,6 +86,8 @@ void IterativeRobotBase::TestPeriodic() { } void IterativeRobotBase::LoopFunc() { + m_watchdog.Reset(); + // Call the appropriate function depending upon the current robot mode if (IsDisabled()) { // Call DisabledInit() if we are now just entering disabled mode from @@ -86,43 +95,73 @@ void IterativeRobotBase::LoopFunc() { if (m_lastMode != Mode::kDisabled) { LiveWindow::GetInstance()->SetEnabled(false); DisabledInit(); + m_watchdog.AddEpoch("DisabledInit()"); m_lastMode = Mode::kDisabled; } + HAL_ObserveUserProgramDisabled(); DisabledPeriodic(); + m_watchdog.AddEpoch("DisabledPeriodic()"); } else if (IsAutonomous()) { // Call AutonomousInit() if we are now just entering autonomous mode from // either a different mode or from power-on. if (m_lastMode != Mode::kAutonomous) { LiveWindow::GetInstance()->SetEnabled(false); AutonomousInit(); + m_watchdog.AddEpoch("AutonomousInit()"); m_lastMode = Mode::kAutonomous; } + HAL_ObserveUserProgramAutonomous(); AutonomousPeriodic(); + m_watchdog.AddEpoch("AutonomousPeriodic()"); } else if (IsOperatorControl()) { // Call TeleopInit() if we are now just entering teleop mode from // either a different mode or from power-on. if (m_lastMode != Mode::kTeleop) { LiveWindow::GetInstance()->SetEnabled(false); TeleopInit(); + m_watchdog.AddEpoch("TeleopInit()"); m_lastMode = Mode::kTeleop; Scheduler::GetInstance()->SetEnabled(true); } + HAL_ObserveUserProgramTeleop(); TeleopPeriodic(); + m_watchdog.AddEpoch("TeleopPeriodic()"); } else { // Call TestInit() if we are now just entering test mode from // either a different mode or from power-on. if (m_lastMode != Mode::kTest) { LiveWindow::GetInstance()->SetEnabled(true); TestInit(); + m_watchdog.AddEpoch("TestInit()"); m_lastMode = Mode::kTest; } + HAL_ObserveUserProgramTest(); TestPeriodic(); + m_watchdog.AddEpoch("TestPeriodic()"); } + RobotPeriodic(); + m_watchdog.AddEpoch("RobotPeriodic()"); + m_watchdog.Disable(); SmartDashboard::UpdateValues(); + LiveWindow::GetInstance()->UpdateValues(); + + // Warn on loop time overruns + if (m_watchdog.IsExpired()) { + m_watchdog.PrintEpochs(); + } +} + +void IterativeRobotBase::PrintLoopOverrunMessage() { + wpi::SmallString<128> str; + wpi::raw_svector_ostream buf(str); + + buf << "Loop time of " << m_period << "s overrun\n"; + + DriverStation::ReportWarning(str); } diff --git a/wpilibc/src/main/native/cpp/TimedRobot.cpp b/wpilibc/src/main/native/cpp/TimedRobot.cpp index 4d0939927c..55380bd6a2 100644 --- a/wpilibc/src/main/native/cpp/TimedRobot.cpp +++ b/wpilibc/src/main/native/cpp/TimedRobot.cpp @@ -23,8 +23,6 @@ void TimedRobot::StartCompetition() { // Tell the DS that the robot is ready to be enabled HAL_ObserveUserProgramStarting(); - m_startLoop = true; - m_expirationTime = Timer::GetFPGATimestamp() + m_period; UpdateAlarm(); @@ -43,18 +41,9 @@ void TimedRobot::StartCompetition() { } } -void TimedRobot::SetPeriod(double period) { - m_period = period; - - if (m_startLoop) { - m_expirationTime = Timer::GetFPGATimestamp() + period; - UpdateAlarm(); - } -} - double TimedRobot::GetPeriod() const { return m_period; } -TimedRobot::TimedRobot() { +TimedRobot::TimedRobot(double period) : IterativeRobotBase(period) { int32_t status = 0; m_notifier = HAL_InitializeNotifier(&status); wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); diff --git a/wpilibc/src/main/native/cpp/Watchdog.cpp b/wpilibc/src/main/native/cpp/Watchdog.cpp new file mode 100644 index 0000000000..c82ba1ff2f --- /dev/null +++ b/wpilibc/src/main/native/cpp/Watchdog.cpp @@ -0,0 +1,59 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "Watchdog.h" + +#include + +#include "Timer.h" + +using namespace frc; + +Watchdog::Watchdog(double timeout, std::function callback) + : m_timeout(timeout), + m_callback(callback), + m_notifier(&Watchdog::TimeoutFunc, this) { + Enable(); +} + +double Watchdog::GetTime() const { + return Timer::GetFPGATimestamp() - m_startTime; +} + +bool Watchdog::IsExpired() const { return m_isExpired; } + +void Watchdog::AddEpoch(wpi::StringRef epochName) { + double currentTime = Timer::GetFPGATimestamp(); + m_epochs[epochName] = currentTime - m_startTime; + m_startTime = currentTime; +} + +void Watchdog::PrintEpochs() { + for (const auto& epoch : m_epochs) { + wpi::outs() << "\t" << epoch.getKey() << ": " << epoch.getValue() << "s\n"; + } +} + +void Watchdog::Reset() { Enable(); } + +void Watchdog::Enable() { + m_startTime = Timer::GetFPGATimestamp(); + m_isExpired = false; + m_epochs.clear(); + m_notifier.StartPeriodic(m_timeout); +} + +void Watchdog::Disable() { m_notifier.Stop(); } + +void Watchdog::TimeoutFunc() { + if (!m_isExpired) { + wpi::outs() << "Watchdog not fed after " << m_timeout << "s\n"; + m_callback(); + m_isExpired = true; + Disable(); + } +} diff --git a/wpilibc/src/main/native/include/IterativeRobotBase.h b/wpilibc/src/main/native/include/IterativeRobotBase.h index 9da3d0cd5c..82c3842267 100644 --- a/wpilibc/src/main/native/include/IterativeRobotBase.h +++ b/wpilibc/src/main/native/include/IterativeRobotBase.h @@ -8,6 +8,7 @@ #pragma once #include "RobotBase.h" +#include "Watchdog.h" namespace frc { @@ -134,14 +135,26 @@ class IterativeRobotBase : public RobotBase { virtual void TestPeriodic(); protected: + /** + * Constructor for IterativeRobotBase. + * + * @param period Period in seconds. + */ + explicit IterativeRobotBase(double period); + virtual ~IterativeRobotBase() = default; void LoopFunc(); + double m_period; + private: enum class Mode { kNone, kDisabled, kAutonomous, kTeleop, kTest }; Mode m_lastMode = Mode::kNone; + Watchdog m_watchdog; + + void PrintLoopOverrunMessage(); }; } // namespace frc diff --git a/wpilibc/src/main/native/include/TimedRobot.h b/wpilibc/src/main/native/include/TimedRobot.h index 9dc77a27d0..383847de15 100644 --- a/wpilibc/src/main/native/include/TimedRobot.h +++ b/wpilibc/src/main/native/include/TimedRobot.h @@ -33,36 +33,27 @@ class TimedRobot : public IterativeRobotBase, public ErrorBase { void StartCompetition() override; /** - * Set time period between calls to Periodic() functions. - * - * A timer event is queued for periodic event notification. Each time the - * interrupt occurs, the event will be immediately requeued for the same time - * interval. - * - * @param period Period in seconds. - */ - void SetPeriod(double seconds); - - /** - * Get time period between calls to Periodic() functions. + * Get the time period between calls to Periodic() functions. */ double GetPeriod() const; - protected: - TimedRobot(); + /** + * Constructor for TimedRobot. + * + * @param period Period in seconds. + */ + explicit TimedRobot(double period = kDefaultPeriod); + ~TimedRobot() override; private: - // Prevents loop from starting if user calls SetPeriod() in RobotInit() - bool m_startLoop = false; - HAL_NotifierHandle m_notifier{0}; // The absolute expiration time double m_expirationTime = 0; // The relative time - double m_period = kDefaultPeriod; + double m_period; /** * Update the HAL alarm time. diff --git a/wpilibc/src/main/native/include/Watchdog.h b/wpilibc/src/main/native/include/Watchdog.h new file mode 100644 index 0000000000..b6006e7bf7 --- /dev/null +++ b/wpilibc/src/main/native/include/Watchdog.h @@ -0,0 +1,92 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include +#include + +#include "Notifier.h" + +namespace frc { + +/** + * A class that's a wrapper around a watchdog timer. + * + * When the timer expires, a message is printed to the console and an optional + * user-provided callback is invoked. + * + * The watchdog is initialized disabled, so the user needs to call Enable() + * before use. + */ +class Watchdog { + public: + /** + * Watchdog constructor. + * + * @param timeout The watchdog's timeout in seconds. + * @param callback This function is called when the timeout expires. + */ + explicit Watchdog(double timeout, std::function callback = [] {}); + + Watchdog(const Watchdog&) = delete; + Watchdog& operator=(const Watchdog&) = delete; + + /** + * Get the time in seconds since the watchdog was last fed. + */ + double GetTime() const; + + /** + * Returns true if the watchdog timer has expired. + */ + bool IsExpired() const; + + /** + * Adds time since last epoch to the list printed by PrintEpochs(). + * + * @param epochName The name to associate with the epoch. + */ + void AddEpoch(wpi::StringRef epochName); + + /** + * Prints list of epochs added so far and their times. + */ + void PrintEpochs(); + + /** + * Resets the watchdog timer. + * + * This also enables the timer if it was previously disabled. + */ + void Reset(); + + /** + * Enables the watchdog timer. + */ + void Enable(); + + /** + * Disable the watchdog. + */ + void Disable(); + + private: + double m_timeout; + std::function m_callback; + Notifier m_notifier; + + double m_startTime = 0.0; + wpi::StringMap m_epochs; + bool m_isExpired = false; + + void TimeoutFunc(); +}; + +} // namespace frc diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobot.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobot.java index 9473e9db3c..1f69951256 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobot.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobot.java @@ -20,7 +20,14 @@ import edu.wpi.first.wpilibj.hal.HAL; * the driver station. */ public class IterativeRobot extends IterativeRobotBase { + private static final double kPacketPeriod = 0.02; + + /** + * Create a new IterativeRobot. + */ public IterativeRobot() { + super(kPacketPeriod); + HAL.report(tResourceType.kResourceType_Framework, tInstances.kFramework_Iterative); } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java index 31d86e9e7b..11a9b17c07 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java @@ -42,6 +42,8 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; */ @SuppressWarnings("PMD.TooManyMethods") public abstract class IterativeRobotBase extends RobotBase { + protected double m_period; + private enum Mode { kNone, kDisabled, @@ -51,6 +53,17 @@ public abstract class IterativeRobotBase extends RobotBase { } private Mode m_lastMode = Mode.kNone; + private final Watchdog m_watchdog; + + /** + * Constructor for IterativeRobotBase. + * + * @param period Period in seconds. + */ + protected IterativeRobotBase(double period) { + m_period = period; + m_watchdog = new Watchdog(period, this::printLoopOverrunMessage); + } /** * Provide an alternate "main loop" via startCompetition(). @@ -179,53 +192,77 @@ public abstract class IterativeRobotBase extends RobotBase { } protected void loopFunc() { + m_watchdog.reset(); + // Call the appropriate function depending upon the current robot mode if (isDisabled()) { - // call DisabledInit() if we are now just entering disabled mode from - // either a different mode or from power-on + // Call DisabledInit() if we are now just entering disabled mode from either a different mode + // or from power-on. if (m_lastMode != Mode.kDisabled) { LiveWindow.setEnabled(false); disabledInit(); + m_watchdog.addEpoch("disabledInit()"); m_lastMode = Mode.kDisabled; } + HAL.observeUserProgramDisabled(); disabledPeriodic(); + m_watchdog.addEpoch("disablePeroidic()"); } else if (isAutonomous()) { - // call Autonomous_Init() if this is the first time - // we've entered autonomous_mode + // Call AutonomousInit() if we are now just entering autonomous mode from either a different + // mode or from power-on. if (m_lastMode != Mode.kAutonomous) { LiveWindow.setEnabled(false); - // KBS NOTE: old code reset all PWMs and relays to "safe values" - // whenever entering autonomous mode, before calling - // "Autonomous_Init()" autonomousInit(); + m_watchdog.addEpoch("autonomousInit()"); m_lastMode = Mode.kAutonomous; } + HAL.observeUserProgramAutonomous(); autonomousPeriodic(); + m_watchdog.addEpoch("autonomousPeriodic()"); } else if (isOperatorControl()) { - // call Teleop_Init() if this is the first time - // we've entered teleop_mode + // Call TeleopInit() if we are now just entering teleop mode from either a different mode or + // from power-on. if (m_lastMode != Mode.kTeleop) { LiveWindow.setEnabled(false); teleopInit(); + m_watchdog.addEpoch("teleopInit()"); m_lastMode = Mode.kTeleop; } + HAL.observeUserProgramTeleop(); teleopPeriodic(); + m_watchdog.addEpoch("teleopPeriodic()"); } else { - // call TestInit() if we are now just entering test mode from either - // a different mode or from power-on + // Call TestInit() if we are now just entering test mode from either a different mode or from + // power-on. if (m_lastMode != Mode.kTest) { LiveWindow.setEnabled(true); testInit(); + m_watchdog.addEpoch("testInit()"); m_lastMode = Mode.kTest; } + HAL.observeUserProgramTest(); testPeriodic(); + m_watchdog.addEpoch("testPeriodic()"); } + robotPeriodic(); + m_watchdog.addEpoch("robotPeriodic()"); + m_watchdog.disable(); SmartDashboard.updateValues(); + LiveWindow.updateValues(); + + // Warn on loop time overruns + if (m_watchdog.isExpired()) { + m_watchdog.printEpochs(); + } + } + + private void printLoopOverrunMessage() { + DriverStation.reportWarning("Loop time of " + m_period + "s overrun\n", false); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java index 5ad6011142..cfadf54302 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java @@ -22,9 +22,6 @@ import edu.wpi.first.wpilibj.hal.NotifierJNI; public class TimedRobot extends IterativeRobotBase { public static final double kDefaultPeriod = 0.02; - // Prevents loop from starting if user calls setPeriod() in robotInit() - private boolean m_startLoop; - // The C pointer to the notifier object. We don't use it directly, it is // just passed to the JNI bindings. private final int m_notifier = NotifierJNI.initializeNotifier(); @@ -32,9 +29,23 @@ public class TimedRobot extends IterativeRobotBase { // The absolute expiration time private double m_expirationTime; - private double m_period = kDefaultPeriod; + private double m_period; + + /** + * Constructor for TimedRobot. + */ + protected TimedRobot() { + this(kDefaultPeriod); + } + + /** + * Constructor for TimedRobot. + * + * @param period Period in seconds. + */ + protected TimedRobot(double period) { + super(period); - public TimedRobot() { // HAL.report(tResourceType.kResourceType_Framework, tInstances.kFramework_Periodic); HAL.report(tResourceType.kResourceType_Framework, tInstances.kFramework_Iterative); } @@ -56,8 +67,6 @@ public class TimedRobot extends IterativeRobotBase { // Tell the DS that the robot is ready to be enabled HAL.observeUserProgramStarting(); - m_startLoop = true; - m_expirationTime = RobotController.getFPGATime() * 1e-6 + m_period; updateAlarm(); @@ -75,20 +84,6 @@ public class TimedRobot extends IterativeRobotBase { } } - /** - * Set time period between calls to Periodic() functions. - * - * @param period Period in seconds. - */ - public void setPeriod(double period) { - m_period = period; - - if (m_startLoop) { - m_expirationTime = RobotController.getFPGATime() * 1e-6 + period; - updateAlarm(); - } - } - /** * Get time period between calls to Periodic() functions. */ diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java new file mode 100644 index 0000000000..4a458d310d --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java @@ -0,0 +1,122 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj; + +import java.util.HashMap; +import java.util.Map; + +/** + * A class that's a wrapper around a watchdog timer. + * + *

When the timer expires, a message is printed to the console and an optional user-provided + * callback is invoked. + * + *

The watchdog is initialized disabled, so the user needs to call enable() before use. + */ +public class Watchdog { + private double m_timeout; + private Runnable m_callback; + private Notifier m_notifier; + + private double m_startTime; + @SuppressWarnings("PMD.UseConcurrentHashMap") + private final Map m_epochs = new HashMap<>(); + boolean m_isExpired; + + /** + * Watchdog constructor. + * + * @param timeout The watchdog's timeout in seconds. + */ + public Watchdog(double timeout) { + this(timeout, () -> { + }); + } + + /** + * Watchdog constructor. + * + * @param timeout The watchdog's timeout in seconds. + * @param callback This function is called when the timeout expires. + */ + public Watchdog(double timeout, Runnable callback) { + m_timeout = timeout; + m_callback = callback; + m_notifier = new Notifier(this::timeoutFunc); + enable(); + } + + /** + * Get the time in seconds since the watchdog was last fed. + */ + public double getTime() { + return Timer.getFPGATimestamp() - m_startTime; + } + + /** + * Returns true if the watchdog timer has expired. + */ + public boolean isExpired() { + return m_isExpired; + } + + /** + * Adds time since last epoch to the list printed by printEpochs(). + * + * @param epochName The name to associate with the epoch. + */ + public void addEpoch(String epochName) { + double currentTime = Timer.getFPGATimestamp(); + m_epochs.put(epochName, currentTime - m_startTime); + m_startTime = currentTime; + } + + /** + * Prints list of epochs added so far and their times. + */ + public void printEpochs() { + m_epochs.forEach((key, value) -> { + System.out.println("\t" + key + ": " + value + "s"); + }); + } + + /** + * Resets the watchdog timer. + * + *

This also enables the timer if it was previously disabled. + */ + public void reset() { + enable(); + } + + /** + * Enables the watchdog timer. + */ + public void enable() { + m_startTime = Timer.getFPGATimestamp(); + m_isExpired = false; + m_epochs.clear(); + m_notifier.startPeriodic(m_timeout); + } + + /** + * Disable the watchdog. + */ + public void disable() { + m_notifier.stop(); + } + + private void timeoutFunc() { + if (!m_isExpired) { + System.out.println("Watchdog not fed after " + m_timeout + "s"); + m_callback.run(); + m_isExpired = true; + disable(); + } + } +}