Add DS loop timing telemetry to magicbot

IterativeRobotBase does this, so it might be useful to do this too.

Author: auscompgeek

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
 
@@ -318,6 +318,7 @@ def disabled(self):
                     ds_attached = not ds_attached
                     self.__nt.putBoolean("is_ds_attached", ds_attached)
 
+                hal.observeUserProgramDisabled()
                 try:
                     self.disabledPeriodic()
                 except:
@@ -352,6 +353,7 @@ def operatorControl(self):
 
         with NotifierDelay(self.control_loop_wait_time) as delay:
             while self.isOperatorControl() and self.isEnabled():
+                hal.observeUserProgramTeleop()
                 try:
                     self.teleopPeriodic()
                 except:
@@ -379,6 +381,7 @@ def test(self):
 
         with NotifierDelay(self.control_loop_wait_time) as delay:
             while self.isTest() and self.isEnabled():
+                hal.observeUserProgramTest()
                 try:
                     self.testPeriodic()
                 except:

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:
@@ -240,6 +240,7 @@ def run(self, control_loop_wait_time=0.020, iter_fn=None, on_exception=None):
 
         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: