def loopFunc(self): """Call the appropriate function depending upon the current robot mode""" if self.isDisabled(): if self.last_mode is not self.Mode.kDisabled: LiveWindow.setEnabled(False) self.disabledInit() self.last_mode = self.Mode.kDisabled hal.observeUserProgramDisabled() self.disabledPeriodic() elif self.isAutonomous(): if self.last_mode is not self.Mode.kAutonomous: LiveWindow.setEnabled(False) self.autonomousInit() self.last_mode = self.Mode.kAutonomous hal.observeUserProgramAutonomous() self.autonomousPeriodic() elif self.isOperatorControl(): if self.last_mode is not self.Mode.kTeleop: LiveWindow.setEnabled(False) self.teleopInit() self.last_mode = self.Mode.kTeleop hal.observeUserProgramTeleop() self.teleopPeriodic() else: if self.last_mode is not self.Mode.kTest: LiveWindow.setEnabled(False) self.testInit() self.last_mode = self.Mode.kTest hal.observeUserProgramTest() self.testPeriodic() self.robotPeriodic() SmartDashboard.updateValues() LiveWindow.updateValues()
def _run(self): """Provides the service routine for the DS polling thread.""" safetyCounter = 0 while self.threadKeepAlive: hal.waitForDSData() self._getData() if self.isDisabled(): safetyCounter = 0 safetyCounter += 1 if safetyCounter >= 4: MotorSafety.checkMotors() safetyCounter = 0 if self.userInDisabled: hal.observeUserProgramDisabled() if self.userInAutonomous: hal.observeUserProgramAutonomous() if self.userInTeleop: hal.observeUserProgramTeleop() if self.userInTest: hal.observeUserProgramTest()
def _run(self): """Provides the service routine for the DS polling thread.""" safetyCounter = 0 while self.threadKeepAlive: hal.waitForDSData() self._getData() with self.dataCond: self.waitForDataPredicate = True self.dataCond.notify_all() #with self.mutex: # dataCond already holds the mutex self.newControlData = True safetyCounter += 1 if safetyCounter >= 4: MotorSafety.checkMotors() safetyCounter = 0 if self.userInDisabled: hal.observeUserProgramDisabled() if self.userInAutonomous: hal.observeUserProgramAutonomous() if self.userInTeleop: hal.observeUserProgramTeleop() if self.userInTest: hal.observeUserProgramTest()
def _run(self) -> None: """Provides the service routine for the DS polling thread.""" safetyCounter = 0 while self.threadKeepAlive: hal.waitForDSData() self._getData() if self.isDisabled(): safetyCounter = 0 safetyCounter += 1 if safetyCounter >= 4: MotorSafety.checkMotors() safetyCounter = 0 if self.userInDisabled: hal.observeUserProgramDisabled() if self.userInAutonomous: hal.observeUserProgramAutonomous() if self.userInTeleop: hal.observeUserProgramTeleop() if self.userInTest: hal.observeUserProgramTest()
def loopFunc(self) -> None: """Call the appropriate function depending upon the current robot mode""" self.watchdog.reset() isEnabled, isAutonomous, isTest = self.getControlState() if not isEnabled: if self.last_mode is not self.Mode.kDisabled: LiveWindow.setEnabled(False) Shuffleboard.disableActuatorWidgets() self.disabledInit() self.watchdog.addEpoch("disabledInit()") self.last_mode = self.Mode.kDisabled hal.observeUserProgramDisabled() self.disabledPeriodic() self.watchdog.addEpoch("disabledPeriodic()") elif isAutonomous: if self.last_mode is not self.Mode.kAutonomous: LiveWindow.setEnabled(False) Shuffleboard.disableActuatorWidgets() self.autonomousInit() self.watchdog.addEpoch("autonomousInit()") self.last_mode = self.Mode.kAutonomous hal.observeUserProgramAutonomous() self.autonomousPeriodic() self.watchdog.addEpoch("autonomousPeriodic()") elif not isTest: if self.last_mode is not self.Mode.kTeleop: LiveWindow.setEnabled(False) Shuffleboard.disableActuatorWidgets() self.teleopInit() self.watchdog.addEpoch("teleopInit()") self.last_mode = self.Mode.kTeleop hal.observeUserProgramTeleop() self.teleopPeriodic() self.watchdog.addEpoch("teleopPeriodic()") else: if self.last_mode is not self.Mode.kTest: LiveWindow.setEnabled(True) Shuffleboard.enableActuatorWidgets() self.testInit() self.watchdog.addEpoch("testInit()") self.last_mode = self.Mode.kTest hal.observeUserProgramTest() self.testPeriodic() self.watchdog.addEpoch("testPeriodic()") self.robotPeriodic() self.watchdog.addEpoch("robotPeriodic()") self.watchdog.disable() SmartDashboard.updateValues() LiveWindow.updateValues() Shuffleboard.update() if self.watchdog.isExpired(): self.watchdog.printEpochs()
def startCompetition(self): hal.report(hal.UsageReporting.kResourceType_Framework, hal.UsageReporting.kFramework_Iterative) self.robotInit() # Tell the DS that the robot is ready to be enabled hal.observeUserProgramStarting() LiveWindow.setEnabled(False) while True: # Wait for new data to arrive self.ds.waitForData() if self.isDisabled(): self.earlyStop = False if self.iterator: self.iterator.close() self.iterator = None LiveWindow.setEnabled(False) hal.observeUserProgramDisabled() else: # not disabled if not self.iterator and not self.earlyStop: LiveWindow.setEnabled(True) try: if self.isTest(): self.iterator = self.test() elif self.isAutonomous(): self.iterator = self.autonomous() else: self.iterator = self.teleop() except: print("Exception while starting sequence!") traceback.print_exc() self.earlyStop = True if self.isTest(): hal.observeUserProgramTest() elif self.isAutonomous(): hal.observeUserProgramAutonomous() else: hal.observeUserProgramTeleop() if self.iterator: try: next(self.iterator) except StopIteration: print("Robot done.") self.iterator = None self.earlyStop = True except: print("Exception in robot code!") traceback.print_exc() self.iterator = None self.earlyStop = True
def startCompetition(self): """ Provide an alternate "main loop" via startCompetition(). Rewritten from IterativeRobot for readability and to initialize scheduler. """ hal.report(hal.HALUsageReporting.kResourceType_Framework, hal.HALUsageReporting.kFramework_Iterative) self.scheduler = Scheduler.getInstance() self.robotInit() # Tell the DS that the robot is ready to be enabled hal.observeUserProgramStarting() # loop forever, calling the appropriate mode-dependent function while True: if self.ds.isDisabled(): hal.observeUserProgramDisabled() self.disabledInit() while self.ds.isDisabled(): self.disabledPeriodic() self.ds.waitForData() elif self.ds.isAutonomous(): hal.observeUserProgramAutonomous() self.autonomousInit() while self.ds.isEnabled() and self.ds.isAutonomous(): self.autonomousPeriodic() self.ds.waitForData() elif self.ds.isTest(): hal.observeUserProgramTest() LiveWindow.setEnabled(True) self.testInit() while self.ds.isEnabled() and self.ds.isTest(): self.testPeriodic() self.ds.waitForData() LiveWindow.setEnabled(False) else: hal.observeUserProgramTeleop() self.teleopInit() # isOperatorControl checks "not autonomous or test", so we need # to check isEnabled as well, since otherwise it will continue # looping while disabled. while self.ds.isEnabled() and self.ds.isOperatorControl(): self.teleopPeriodic() self.ds.waitForData()
def loopFunc(self): """This version of loopFunc passes the event loop to all init() and periodic() functions.""" if self.isDisabled(): if self.last_mode is not self.Mode.kDisabled: LiveWindow.setEnabled(False) self._flush_commands() self.disabledInit() self.last_mode = self.Mode.kDisabled hal.observeUserProgramDisabled() self.disabledPeriodic() elif self.isAutonomous(): if self.last_mode is not self.Mode.kAutonomous: LiveWindow.setEnabled(False) self._flush_commands() self.autonomousInit() self.last_mode = self.Mode.kAutonomous hal.observeUserProgramAutonomous() self.autonomousPeriodic() elif self.isOperatorControl(): if self.last_mode is not self.Mode.kTeleop: LiveWindow.setEnabled(False) self._flush_commands() self.teleopInit() self.last_mode = self.Mode.kTeleop hal.observeUserProgramTeleop() self.teleopPeriodic() else: if self.last_mode is not self.Mode.kTest: LiveWindow.setEnabled(True) self._flush_commands() self.testInit() self.last_mode = self.Mode.kTest hal.observeUserProgramTest() self.testPeriodic() self.robotPeriodic() self._run_commands() SmartDashboard.updateValues() LiveWindow.updateValues()
def startCompetition(self): """Provide an alternate "main loop" via startCompetition().""" hal.report(hal.UsageReporting.kResourceType_Framework, hal.UsageReporting.kFramework_Iterative) self.robotInit() # Tell the DS that the robot is ready to be enabled hal.observeUserProgramStarting() # loop forever, calling the appropriate mode-dependent function LiveWindow.setEnabled(False) while True: # Wait for new data to arrive self.ds.waitForData() # Call the appropriate function depending upon the current robot mode if self.isDisabled(): # call DisabledInit() if we are now just entering disabled mode from # either a different mode or from power-on if not self.disabledInitialized: LiveWindow.setEnabled(False) self.disabledInit() self.disabledInitialized = True # reset the initialization flags for the other modes self.autonomousInitialized = False self.teleopInitialized = False self.testInitialized = False hal.observeUserProgramDisabled() self.disabledPeriodic() elif self.isTest(): # call TestInit() if we are now just entering test mode from either # a different mode or from power-on if not self.testInitialized: LiveWindow.setEnabled(True) self.testInit() self.testInitialized = True self.autonomousInitialized = False self.teleopInitialized = False self.disabledInitialized = False hal.observeUserProgramTest() self.testPeriodic() elif self.isAutonomous(): # call Autonomous_Init() if this is the first time # we've entered autonomous_mode if not self.autonomousInitialized: LiveWindow.setEnabled(False) # KBS NOTE: old code reset all PWMs and relays to "safe values" # whenever entering autonomous mode, before calling # "Autonomous_Init()" self.autonomousInit() self.autonomousInitialized = True self.testInitialized = False self.teleopInitialized = False self.disabledInitialized = False hal.observeUserProgramAutonomous() self.autonomousPeriodic() else: # call Teleop_Init() if this is the first time # we've entered teleop_mode if not self.teleopInitialized: LiveWindow.setEnabled(False) self.teleopInit() self.teleopInitialized = True self.testInitialized = False self.autonomousInitialized = False self.disabledInitialized = False hal.observeUserProgramTeleop() self.teleopPeriodic() self.robotPeriodic()
def startCompetition(self): self.robotInit() # Tell the DS that the robot is ready to be enabled hal.observeUserProgramStarting() self._expirationTime = RobotController.getFPGATime( ) * 1e-6 + self.period self._updateAlarm() while True: if hal.waitForNotifierAlarm(self._notifier[0]) == 0: if self.iterator is not None: self.iterator.close() self.iterator = None self.logger.error("Robot Break!") break self._expirationTime += self.period self._updateAlarm() # Wait for new data to arrive self.ds.waitForData() if self.isDisabled(): self.earlyStop = False if self.iterator: self.iterator.close() self.iterator = None hal.observeUserProgramDisabled() else: # not disabled if not self.iterator and not self.earlyStop: try: if self.isTest(): self.iterator = self.test() elif self.isAutonomous(): self.iterator = self.autonomous() else: self.iterator = self.teleop() if self.iterator is None: self.logger.error( "Robot function is not a generator!") except BaseException as e: self.logger.exception( "Exception while starting sequence!", exc_info=e) self.earlyStop = True if self.isTest(): hal.observeUserProgramTest() elif self.isAutonomous(): hal.observeUserProgramAutonomous() else: hal.observeUserProgramTeleop() if self.iterator: try: next(self.iterator) except StopIteration: self.logger.info("Robot done.") self.iterator = None self.earlyStop = True except BaseException as e: self.logger.exception("Exception in robot code!", exc_info=e) self.iterator = None self.earlyStop = True