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):
"""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 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
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()
