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