Add loop timing to IterativeRobot and TimedRobot (#781)

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);
 }

wpilibc/src/main/native/cpp/IterativeRobotBase.cpp

@@ -10,14 +10,21 @@
 #include <cstdio>
 
 #include <HAL/HAL.h>
+#include <wpi/SmallString.h>
 #include <wpi/raw_ostream.h>
 
 #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);
 }

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));

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 <wpi/raw_ostream.h>
+
+#include "Timer.h"
+
+using namespace frc;
+
+Watchdog::Watchdog(double timeout, std::function<void()> 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();
+  }
+}

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

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.

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 <functional>
+
+#include <wpi/StringMap.h>
+#include <wpi/StringRef.h>
+
+#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<void()> 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<void()> m_callback;
+  Notifier m_notifier;
+
+  double m_startTime = 0.0;
+  wpi::StringMap<double> m_epochs;
+  bool m_isExpired = false;
+
+  void TimeoutFunc();
+};
+
+}  // namespace frc

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);
   }
 

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);
   }
 }

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.
    */

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.
+ *
+ * <p>When the timer expires, a message is printed to the console and an optional user-provided
+ * callback is invoked.
+ *
+ * <p>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<String, Double> 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.
+   *
+   * <p>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();
+    }
+  }
+}