class Robot(wpilib.IterativeRobot):
def robotInit(self):
constants.load_control_config()
wpilib.CameraServer.launch('driver_vision.py:main')
self.autoPositionSelect = wpilib.SendableChooser()
self.autoPositionSelect.addDefault('Middle', 'Middle')
self.autoPositionSelect.addObject('Left', 'Left')
self.autoPositionSelect.addObject('Right', 'Right')
wpilib.SmartDashboard.putData('Robot Starting Position',
self.autoPositionSelect)
self.control_stick = wpilib.Joystick(0)
self.drivetrain = swerve.SwerveDrive(constants.chassis_length,
constants.chassis_width,
constants.swerve_config)
self.lift = lift.RD4BLift(constants.lift_ids['left'],
constants.lift_ids['right'])
self.winch = winch.Winch(constants.winch_id)
# self.claw = lift.Claw(
# constants.claw_id
# )
self.imu = IMU(wpilib.SPI.Port.kMXP)
def disabledInit(self):
# We don't really _need_ to reload configuration in
# every init call-- it's just useful for debugging.
# (no need to restart robot code just to load new values)
self.drivetrain.load_config_values()
def disabledPeriodic(self):
self.drivetrain.update_smart_dashboard()
self.imu.update_smart_dashboard()
def autonomousInit(self):
self.drivetrain.load_config_values()
self.auto = Autonomous(self, self.autoPositionSelect.getSelected())
self.auto.periodic()
def autonomousPeriodic(self):
self.auto.update_smart_dashboard()
self.imu.update_smart_dashboard()
self.drivetrain.update_smart_dashboard()
self.auto.periodic()
def teleopInit(self):
self.teleop = Teleop(self, self.control_stick)
self.drivetrain.load_config_values()
constants.load_control_config()
def teleopPeriodic(self):
# For now: basic driving
constants.load_control_config()
self.teleop.drive()
self.teleop.buttons()
self.teleop.lift_control()
self.drivetrain.update_smart_dashboard()
self.teleop.update_smart_dashboard()
self.imu.update_smart_dashboard()
class Robot(wpilib.IterativeRobot):
def robotInit(self):
constants.load_control_config()
wpilib.CameraServer.launch('driver_vision.py:main')
self.autoPositionSelect = wpilib.SendableChooser()
self.autoPositionSelect.addDefault('Middle-Baseline',
'Middle-Baseline')
self.autoPositionSelect.addObject('Middle-Placement',
'Middle-Placement') # noqa: E501
self.autoPositionSelect.addObject('Left', 'Left')
self.autoPositionSelect.addObject('Right', 'Right')
wpilib.SmartDashboard.putData('Robot Starting Position',
self.autoPositionSelect)
self.drivetrain = swerve.SwerveDrive(constants.chassis_length,
constants.chassis_width,
constants.swerve_config)
self.drivetrain.load_config_values()
self.lift = lift.ManualControlLift(constants.lift_ids['left'],
constants.lift_ids['right'],
constants.lift_limit_channel,
constants.start_limit_channel)
self.winch = winch.Winch(constants.winch_id)
self.throttle = wpilib.Joystick(1)
self.claw = lift.Claw(constants.claw_id, constants.claw_follower_id)
self.imu = IMU(wpilib.SPI.Port.kMXP)
self.sd_update_timer = wpilib.Timer()
self.sd_update_timer.reset()
self.sd_update_timer.start()
def disabledInit(self):
pass
def disabledPeriodic(self):
try:
self.lift.load_config_values()
self.drivetrain.load_config_values()
except: # noqa: E772
log_exception('disabled', 'when loading config')
try:
self.drivetrain.update_smart_dashboard()
self.imu.update_smart_dashboard()
self.lift.update_smart_dashboard()
self.winch.update_smart_dashboard()
wpilib.SmartDashboard.putNumber(
"Throttle Pos", self.throttle.getRawAxis(constants.liftAxis))
except: # noqa: E772
log_exception('disabled', 'when updating SmartDashboard')
try:
self.lift.checkLimitSwitch()
pass
except: # noqa: E772
log_exception('disabled', 'when checking lift limit switch')
self.drivetrain.update_smart_dashboard()
def autonomousInit(self):
try:
self.drivetrain.load_config_values()
self.lift.load_config_values()
except: # noqa: E772
log_exception('auto-init', 'when loading config')
self.autoPos = None
try:
self.autoPos = self.autoPositionSelect.getSelected()
except: # noqa: E772
self.autoPos = None
log_exception('auto-init', 'when getting robot start position')
try:
if self.autoPos is not None and self.autoPos != 'None':
self.auto = Autonomous(self, self.autoPos)
else:
log('auto-init', 'Disabling autonomous...')
except: # noqa: E772
log_exception('auto-init', 'in Autonomous constructor')
try:
self.lift.checkLimitSwitch()
pass
except: # noqa: E772
log_exception('auto-init', 'when checking lift limit switch')
def autonomousPeriodic(self):
try:
if self.sd_update_timer.hasPeriodPassed(0.5):
self.auto.update_smart_dashboard()
self.imu.update_smart_dashboard()
self.drivetrain.update_smart_dashboard()
self.lift.update_smart_dashboard()
self.winch.update_smart_dashboard()
except: # noqa: E772
log_exception('auto', 'when updating SmartDashboard')
try:
if self.autoPos is not None and self.autoPos != 'None':
self.auto.periodic()
except: # noqa: E772
# Stop everything.
self.drivetrain.immediate_stop()
self.lift.setLiftPower(0)
self.claw.set_power(0)
self.winch.stop()
log_exception('auto', 'in auto :periodic()')
try:
self.lift.checkLimitSwitch()
pass
except: # noqa: E772
log_exception('auto', 'when checking lift limit switch')
def teleopInit(self):
try:
self.teleop = Teleop(self)
except: # noqa: E772
log_exception('teleop-init', 'in Teleop constructor')
try:
self.drivetrain.load_config_values()
self.lift.load_config_values()
constants.load_control_config()
except: # noqa: E772
log_exception('teleop-init', 'when loading config')
try:
self.lift.checkLimitSwitch()
pass
except: # noqa: E772
log_exception('teleop-init', 'when checking lift limit switch')
def teleopPeriodic(self):
try:
self.teleop.drive()
except: # noqa: E772
log_exception('teleop', 'in drive control')
self.drivetrain.immediate_stop()
try:
self.teleop.buttons()
except: # noqa: E772
log_exception('teleop', 'in button handler')
try:
self.teleop.lift_control()
except: # noqa: E772
log_exception('teleop', 'in lift_control')
self.lift.setLiftPower(0)
try:
self.teleop.claw_control()
except: # noqa: E772
log_exception('teleop', 'in claw_control')
self.claw.set_power(0)
try:
self.teleop.winch_control()
except: # noqa: E772
log_exception('teleop', 'in winch_control')
self.winch.stop()
try:
self.lift.checkLimitSwitch()
pass
except: # noqa: E772
log_exception('teleop', 'in lift.checkLimitSwitch')
if self.sd_update_timer.hasPeriodPassed(0.5):
try:
constants.load_control_config()
self.drivetrain.load_config_values()
self.lift.load_config_values()
except: # noqa: E772
log_exception('teleop', 'when loading config')
try:
self.drivetrain.update_smart_dashboard()
self.teleop.update_smart_dashboard()
self.imu.update_smart_dashboard()
self.lift.update_smart_dashboard()
self.winch.update_smart_dashboard()
except: # noqa: E772
log_exception('teleop', 'when updating SmartDashboard')