Merge pull request #127 from auscompgeek/magicbot-telemetry

Add loop timing telemetry to magicbot

Author: virtuald

magicbot/magicrobot.py

@@ -2,14 +2,14 @@
 import inspect
 import logging
 
+import hal
 import wpilib
 
 from networktables import NetworkTables
 from wpilib.shuffleboard import Shuffleboard
 
-from robotpy_ext.misc import NotifierDelay
 from robotpy_ext.autonomous import AutonomousModeSelector
-
+from robotpy_ext.misc import NotifierDelay
 from robotpy_ext.misc.orderedclass import OrderedClass
 from robotpy_ext.misc.annotations import get_class_annotations
 
@@ -34,8 +34,8 @@ class MagicRobot(wpilib.SampleRobot, metaclass=OrderedClass):
 
         MagicRobot uses the :class:`.AutonomousModeSelector` to allow you
         to define multiple autonomous modes and to select one of them via
-        the SmartDashboard/SFX.
-        
+        the SmartDashboard/Shuffleboard.
+
         MagicRobot will set the following NetworkTables variables
         automatically:
         
@@ -78,6 +78,8 @@ def robotInit(self):
         self.__nt.putBoolean("is_simulation", self.isSimulation())
         self.__nt.putBoolean("is_ds_attached", self.ds.isDSAttached())
 
+        self.watchdog = wpilib.Watchdog(self.control_loop_wait_time, self._loop_overrun)
+
     def createObjects(self):
         """
             You should override this and initialize all of your wpilib
@@ -183,9 +185,13 @@ def robotPeriodic(self):
             The default implementation will update
             SmartDashboard, LiveWindow and Shuffleboard.
         """
+        watchdog = self.watchdog
         wpilib.SmartDashboard.updateValues()
+        watchdog.addEpoch("SmartDashboard")
         wpilib.LiveWindow.updateValues()
+        watchdog.addEpoch("LiveWindow")
         Shuffleboard.update()
+        watchdog.addEpoch("Shuffleboard")
 
     def onException(self, forceReport=False):
         """
@@ -289,6 +295,7 @@ def autonomous(self):
             self.control_loop_wait_time,
             (self._execute_components, self._update_feedback, self.robotPeriodic),
             self.onException,
+            watchdog=self.watchdog,
         )
 
         self._on_mode_disable_components()
@@ -301,6 +308,8 @@ def disabled(self):
 
             .. warning:: Internal API, don't override
         """
+        watchdog = self.watchdog
+        watchdog.reset()
 
         self.__nt.putString("mode", "disabled")
         ds_attached = None
@@ -311,21 +320,31 @@ def disabled(self):
             self.disabledInit()
         except:
             self.onException(forceReport=True)
+        watchdog.addEpoch("disabledInit()")
 
         with NotifierDelay(self.control_loop_wait_time) as delay:
             while self.isDisabled():
                 if ds_attached != self.ds.isDSAttached():
                     ds_attached = not ds_attached
                     self.__nt.putBoolean("is_ds_attached", ds_attached)
 
+                hal.observeUserProgramDisabled()
                 try:
                     self.disabledPeriodic()
                 except:
                     self.onException()
+                watchdog.addEpoch("disabledPeriodic()")
 
                 self._update_feedback()
                 self.robotPeriodic()
+                watchdog.addEpoch("robotPeriodic()")
+                watchdog.disable()
+
+                if watchdog.isExpired():
+                    watchdog.printEpochs()
+
                 delay.wait()
+                watchdog.reset()
 
     def operatorControl(self):
         """
@@ -335,6 +354,8 @@ def operatorControl(self):
 
             .. warning:: Internal API, don't override
         """
+        watchdog = self.watchdog
+        watchdog.reset()
 
         self.__nt.putString("mode", "teleop")
         # don't need to update this during teleop -- presumably will switch
@@ -349,24 +370,36 @@ def operatorControl(self):
             self.teleopInit()
         except:
             self.onException(forceReport=True)
+        watchdog.addEpoch("teleopInit()")
 
         with NotifierDelay(self.control_loop_wait_time) as delay:
             while self.isOperatorControl() and self.isEnabled():
+                hal.observeUserProgramTeleop()
                 try:
                     self.teleopPeriodic()
                 except:
                     self.onException()
+                watchdog.addEpoch("teleopPeriodic()")
 
                 self._execute_components()
+
                 self._update_feedback()
                 self.robotPeriodic()
+                watchdog.addEpoch("robotPeriodic()")
+                watchdog.disable()
+
+                if watchdog.isExpired():
+                    watchdog.printEpochs()
 
                 delay.wait()
+                watchdog.reset()
 
         self._on_mode_disable_components()
 
     def test(self):
         """Called when the robot is in test mode"""
+        watchdog = self.watchdog
+        watchdog.reset()
 
         self.__nt.putString("mode", "test")
         self.__nt.putBoolean("is_ds_attached", self.ds.isDSAttached())
@@ -376,33 +409,45 @@ def test(self):
             self.testInit()
         except:
             self.onException(forceReport=True)
+        watchdog.addEpoch("testInit()")
 
         with NotifierDelay(self.control_loop_wait_time) as delay:
             while self.isTest() and self.isEnabled():
+                hal.observeUserProgramTest()
                 try:
                     self.testPeriodic()
                 except:
                     self.onException()
+                watchdog.addEpoch("testPeriodic()")
 
                 self._update_feedback()
                 self.robotPeriodic()
+                watchdog.addEpoch("robotPeriodic()")
+                watchdog.disable()
+
+                if watchdog.isExpired():
+                    watchdog.printEpochs()
+
                 delay.wait()
+                watchdog.reset()
 
     def _on_mode_enable_components(self):
         # initialize things
-        for component in self._components:
-            if hasattr(component, "on_enable"):
+        for _, component in self._components:
+            on_enable = getattr(component, "on_enable", None)
+            if on_enable is not None:
                 try:
-                    component.on_enable()
+                    on_enable()
                 except:
                     self.onException(forceReport=True)
 
     def _on_mode_disable_components(self):
         # deinitialize things
-        for component in self._components:
-            if hasattr(component, "on_disable"):
+        for _, component in self._components:
+            on_disable = getattr(component, "on_disable", None)
+            if on_disable is not None:
                 try:
-                    component.on_disable()
+                    on_disable()
                 except:
                     self.onException(forceReport=True)
 
@@ -492,7 +537,6 @@ def _create_components(self):
 
         # For each new component, perform magic injection
         for cname, component in components:
-            self._components.append(component)
             setup_tunables(component, cname, "components")
             self._feedbacks += collect_feedbacks(component, cname, "components")
             self._setup_vars(cname, component)
@@ -518,6 +562,8 @@ def _create_components(self):
             if hasattr(mode, "setup"):
                 mode.setup()
 
+        self._components = components
+
     def _create_component(self, name, ctyp):
         # Create instance, set it on self
         component = ctyp()
@@ -638,13 +684,19 @@ def _update_feedback(self):
                 self.onException()
                 continue
             entry.setValue(value)
+        self.watchdog.addEpoch("@magicbot.feedback")
 
     def _execute_components(self):
-        for component in self._components:
+        for name, component in self._components:
             try:
                 component.execute()
             except:
                 self.onException()
+            self.watchdog.addEpoch(name)
 
         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

@@ -1,15 +1,15 @@
-from glob import glob
 import importlib
 import inspect
-import os
-
 import logging
+import os
+from glob import glob
 
-logger = logging.getLogger("autonomous")
+import hal
+import wpilib
 
 from ..misc.precise_delay import NotifierDelay
 
-import wpilib
+logger = logging.getLogger("autonomous")
 
 
 class AutonomousModeSelector:
@@ -118,11 +118,11 @@ def __init__(self, autonomous_pkgname, *args, **kwargs):
             #    on the field..
 
             for name, obj in inspect.getmembers(module, inspect.isclass):
-
-                if hasattr(obj, "MODE_NAME"):
+                mode_name = getattr(obj, "MODE_NAME", None)
+                if mode_name is not None:
 
                     # don't allow the driver to select this mode
-                    if hasattr(obj, "DISABLED") and obj.DISABLED:
+                    if getattr(obj, "DISABLED", False):
                         logger.warning(
                             "autonomous mode %s is marked as disabled", obj.MODE_NAME
                         )
@@ -137,16 +137,15 @@ def __init__(self, autonomous_pkgname, *args, **kwargs):
                         else:
                             continue
 
-                    if instance.MODE_NAME in self.modes:
+                    if mode_name in self.modes:
                         if not self.ds.isFMSAttached():
                             raise RuntimeError(
-                                "Duplicate name %s in %s"
-                                % (instance.MODE_NAME, module_filename)
+                                "Duplicate name %s in %s" % (mode_name, module_filename)
                             )
 
                         logger.error(
                             "Duplicate name %s specified by object type %s in module %s",
-                            instance.MODE_NAME,
+                            mode_name,
                             name,
                             module_filename,
                         )
@@ -166,7 +165,7 @@ def __init__(self, autonomous_pkgname, *args, **kwargs):
         logger.info("Loaded autonomous modes:")
         for k, v in sorted(self.modes.items()):
 
-            if hasattr(v, "DEFAULT") and v.DEFAULT == True:
+            if getattr(v, "DEFAULT", False):
                 logger.info(" -> %s [Default]", k)
                 self.chooser.setDefaultOption(k, v)
                 default_modes.append(k)
@@ -198,7 +197,13 @@ def __init__(self, autonomous_pkgname, *args, **kwargs):
 
         logger.info("Autonomous switcher initialized")
 
-    def run(self, control_loop_wait_time=0.020, iter_fn=None, on_exception=None):
+    def run(
+        self,
+        control_loop_wait_time=0.020,
+        iter_fn=None,
+        on_exception=None,
+        watchdog: wpilib.Watchdog = None,
+    ):
         """
             This function does everything required to implement autonomous
             mode behavior. You should call this from your autonomous mode
@@ -215,12 +220,17 @@ def run(self, control_loop_wait_time=0.020, iter_fn=None, on_exception=None):
                             autonomous mode is executing
             :param on_exception: Called when an uncaught exception is raised,
                                  must take a single keyword arg "forceReport"
+            :param watchdog: a WPILib Watchdog to feed every iteration
         """
+        if watchdog:
+            watchdog.reset()
 
         logger.info("Begin autonomous")
 
         if iter_fn is None:
             iter_fn = lambda: None
+        if not isinstance(iter_fn, (list, tuple)):
+            iter_fn = (iter_fn,)
 
         if on_exception is None:
             on_exception = self._on_exception
@@ -233,25 +243,36 @@ def run(self, control_loop_wait_time=0.020, iter_fn=None, on_exception=None):
             self._on_autonomous_enable()
         except:
             on_exception(forceReport=True)
+        if watchdog:
+            watchdog.addEpoch("auto on_enable")
 
         #
         # Autonomous control loop
         #
 
         with NotifierDelay(control_loop_wait_time) as delay:
             while self.ds.isAutonomous() and self.ds.isEnabled():
+                hal.observeUserProgramAutonomous()
                 try:
                     self._on_iteration(timer.get())
                 except:
                     on_exception()
+                if watchdog:
+                    watchdog.addEpoch("auto on_iteration")
+
+                for fn in iter_fn:
+                    fn()
+
+                if watchdog:
+                    watchdog.addEpoch("robotPeriodic()")
+                    watchdog.disable()
 
-                if isinstance(iter_fn, (list, tuple)):
-                    for fn in iter_fn:
-                        fn()
-                else:
-                    iter_fn()
+                    if watchdog.isExpired():
+                        watchdog.printEpochs()
 
                 delay.wait()
+                if watchdog:
+                    watchdog.reset()
 
         #
         # Done with autonomous, finish up
@@ -284,7 +305,7 @@ def _on_autonomous_enable(self):
             self.active_mode = self.chooser.getSelected()
 
         if self.active_mode is not None:
-            logger.info("Enabling '%s'" % self.active_mode.MODE_NAME)
+            logger.info("Enabling '%s'", self.active_mode.MODE_NAME)
             self.active_mode.on_enable()
         else:
             logger.warning(