Add loop timing to IterativeRobot and TimedRobot (#781)

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