Add working SimpleWatchdog

docs/robotpy_ext.misc.rst

@@ -33,3 +33,11 @@ robotpy_ext.misc.periodic_filter module
     :members:
     :undoc-members:
     :show-inheritance:
+
+robotpy_ext.misc.simple_watchdog module
+---------------------------------------
+
+.. automodule:: robotpy_ext.misc.simple_watchdog
+    :members:
+    :undoc-members:
+    :show-inheritance:

magicbot/magicrobot.py

@@ -12,6 +12,7 @@
 from robotpy_ext.misc import NotifierDelay
 from robotpy_ext.misc.orderedclass import OrderedClass
 from robotpy_ext.misc.annotations import get_class_annotations
+from robotpy_ext.misc.simple_watchdog import SimpleWatchdog
 
 from .magic_tunable import setup_tunables, tunable, collect_feedbacks
 from .magic_reset import will_reset_to
@@ -90,7 +91,7 @@ def robotInit(self):
         self.__lv_update = wpilib.LiveWindow.updateValues
         self.__sf_update = Shuffleboard.update
 
-        self.watchdog = wpilib.Watchdog(self.control_loop_wait_time, self._loop_overrun)
+        self.watchdog = SimpleWatchdog(self.control_loop_wait_time)
 
     def createObjects(self):
         """
@@ -360,10 +361,9 @@ def disabled(self):
                 self._update_feedback()
                 self.robotPeriodic()
                 watchdog.addEpoch("robotPeriodic()")
-                watchdog.disable()
+                # watchdog.disable()
 
-                if watchdog.isExpired():
-                    watchdog.printEpochs()
+                watchdog.printIfExpired()
 
                 delay.wait()
                 watchdog.reset()
@@ -409,10 +409,9 @@ def operatorControl(self):
                 self._update_feedback()
                 self.robotPeriodic()
                 watchdog.addEpoch("robotPeriodic()")
-                watchdog.disable()
+                # watchdog.disable()
 
-                if watchdog.isExpired():
-                    watchdog.printEpochs()
+                watchdog.printIfExpired()
 
                 delay.wait()
                 watchdog.reset()
@@ -445,10 +444,9 @@ def test(self):
                 self._update_feedback()
                 self.robotPeriodic()
                 watchdog.addEpoch("robotPeriodic()")
-                watchdog.disable()
+                # watchdog.disable()
 
-                if watchdog.isExpired():
-                    watchdog.printEpochs()
+                watchdog.printIfExpired()
 
                 delay.wait()
                 watchdog.reset()
@@ -720,7 +718,3 @@ def _execute_components(self):
 
         for reset_dict, component in self._reset_components:
             component.__dict__.update(reset_dict)
-
-    def _loop_overrun(self):
-        # TODO: print something here without making it extremely annoying
-        pass

robotpy_ext/autonomous/selector.py

@@ -3,11 +3,13 @@
 import logging
 import os
 from glob import glob
+from typing import Union
 
 import hal
 import wpilib
 
 from ..misc.precise_delay import NotifierDelay
+from ..misc.simple_watchdog import SimpleWatchdog
 
 logger = logging.getLogger("autonomous")
 
@@ -202,7 +204,7 @@ def run(
         control_loop_wait_time=0.020,
         iter_fn=None,
         on_exception=None,
-        watchdog: wpilib.Watchdog = None,
+        watchdog: Union[wpilib.Watchdog, SimpleWatchdog] = None,
     ):
         """
             This function does everything required to implement autonomous
@@ -225,6 +227,13 @@ def run(
         if watchdog:
             watchdog.reset()
 
+            if isinstance(watchdog, SimpleWatchdog):
+                watchdog_check_expired = watchdog.printIfExpired
+            else:
+                def watchdog_check_expired():
+                    if watchdog.isExpired():
+                        watchdog.printEpochs()
+
         logger.info("Begin autonomous")
 
         if iter_fn is None:
@@ -269,8 +278,7 @@ def run(
                     watchdog.addEpoch("robotPeriodic()")
                     watchdog.disable()
 
-                    if watchdog.isExpired():
-                        watchdog.printEpochs()
+                    watchdog_check_expired()
 
                 delay.wait()
                 if watchdog:

robotpy_ext/misc/simple_watchdog.py

@@ -0,0 +1,108 @@
+import hal
+import logging
+
+logger = logging.getLogger("simple_watchdog")
+
+__all__ = ["SimpleWatchdog"]
+
+
+class SimpleWatchdog:
+    """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.
+
+    .. note:: This is a simpler replacement for the :class:`wpilib.Watchdog`,
+              and should function mostly the same (except that this watchdog will
+              not detect infinite loops).
+
+    .. warning:: This watchdog is not threadsafe
+
+    """
+
+    # Used for timeout print rate-limiting
+    kMinPrintPeriod = 1000000  # us
+
+    def __init__(self, timeout: float):
+        """Watchdog constructor.
+
+        :param timeout: The watchdog's timeout in seconds with microsecond resolution.
+        """
+        self._get_time = hal.getFPGATime
+
+        self._startTime = 0  # us
+        self._timeout = int(timeout * 1e6)  # us
+        self._expirationTime = 0  # us
+        self._lastTimeoutPrintTime = 0  # us
+        self._lastEpochsPrintTime = 0  # us
+        self._epochs = []
+
+    def getTime(self) -> float:
+        """Returns the time in seconds since the watchdog was last fed."""
+        return (self._get_time() - self._startTime) / 1e6
+
+    def setTimeout(self, timeout: float) -> None:
+        """Sets the watchdog's timeout.
+
+        :param timeout: The watchdog's timeout in seconds with microsecond
+                        resolution.
+        """
+        self._epochs.clear()
+        timeout = int(timeout * 1e6)  # us
+        self._timeout = timeout
+        self._startTime = self._get_time()
+        self._expirationTime = self._startTime + timeout
+
+    def getTimeout(self) -> float:
+        """Returns the watchdog's timeout in seconds."""
+        return self._timeout / 1e6
+
+    def isExpired(self) -> bool:
+        """Returns true if the watchdog timer has expired."""
+        return self._get_time() > self._expirationTime
+
+    def addEpoch(self, epochName: str) -> None:
+        """
+        Adds time since last epoch to the list printed by printEpochs().
+
+        Epochs are a way to partition the time elapsed so that when
+        overruns occur, one can determine which parts of an operation
+        consumed the most time.
+
+        :param epochName: The name to associate with the epoch.
+        """
+        self._epochs.append((epochName, self._get_time()))
+
+    def printIfExpired(self) -> None:
+        """Prints list of epochs added so far and their times."""
+        now = self._get_time()
+        if (
+            now > self._expirationTime
+            and now - self._lastEpochsPrintTime > self.kMinPrintPeriod
+        ):
+            log = logger.info
+            self._lastEpochsPrintTime = now
+            prev = self._startTime
+            log("Watchdog not fed after %.6fs", (now - prev) / 1e6)
+            for key, value in self._epochs:
+                log("\t%s: %.6fs", key, (value - prev) / 1e6)
+                prev = value
+
+    def reset(self) -> None:
+        """Resets the watchdog timer.
+
+        This also enables the timer if it was previously disabled.
+        """
+        self.enable()
+
+    def enable(self) -> None:
+        """Enables the watchdog timer."""
+        self._epochs.clear()
+        self._startTime = self._get_time()
+        self._expirationTime = self._startTime + self._timeout
+
+    def disable(self) -> None:
+        """Disables the watchdog timer."""
+        # .. this doesn't do anything