Update magicbot for new DriverStation threading model

Author: virtuald

magicbot/magicrobot.py

@@ -434,9 +434,17 @@ def _disabled(self) -> None:
             self.onException(forceReport=True)
         watchdog.addEpoch("disabledInit()")
 
+        refreshData = wpilib.DriverStation.refreshData
+        DSControlWord = wpilib.DSControlWord
+
         with NotifierDelay(self.control_loop_wait_time) as delay:
-            while not self.__done and self.isDisabled():
-                if ds_attached != self.__is_ds_attached():
+            while not self.__done:
+                refreshData()
+                cw = DSControlWord()
+                if cw.isEnabled():
+                    break
+
+                if ds_attached != cw.isDSAttached():
                     ds_attached = not ds_attached
                     self.__nt_put_is_ds_attached(ds_attached)
 
@@ -484,9 +492,15 @@ def _operatorControl(self) -> None:
         watchdog.addEpoch("teleopInit()")
 
         observe = hal.observeUserProgramTeleop
+        refreshData = wpilib.DriverStation.refreshData
+        isTeleopEnabled = wpilib.DriverStation.isTeleopEnabled
 
         with NotifierDelay(self.control_loop_wait_time) as delay:
-            while not self.__done and self.isTeleopEnabled():
+            while not self.__done:
+                refreshData()
+                if not isTeleopEnabled():
+                    break
+
                 observe()
                 try:
                     self.teleopPeriodic()
@@ -526,8 +540,16 @@ def _test(self) -> None:
             self.onException(forceReport=True)
         watchdog.addEpoch("testInit()")
 
+        refreshData = wpilib.DriverStation.refreshData
+        DSControlWord = wpilib.DSControlWord
+
         with NotifierDelay(self.control_loop_wait_time) as delay:
-            while not self.__done and self.isTest() and self.isEnabled():
+            while not self.__done:
+                refreshData()
+                cw = DSControlWord()
+                if not (cw.isTest() and cw.isEnabled()):
+                    break
+
                 hal.observeUserProgramTest()
                 try:
                     self.testPeriodic()

robotpy_ext/autonomous/selector.py

@@ -272,10 +272,15 @@ def watchdog_check_expired():
         #
 
         observe = hal.observeUserProgramAutonomous
+        refreshData = wpilib.DriverStation.refreshData
         isAutonomousEnabled = wpilib.DriverStation.isAutonomousEnabled
 
         with NotifierDelay(control_loop_wait_time) as delay:
-            while isAutonomousEnabled():
+            while True:
+                refreshData()
+                if not isAutonomousEnabled():
+                    break
+
                 observe()
                 try:
                     self._on_iteration(timer.get())