Add loop timing to IterativeRobot and TimedRobot (#781)

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