def disabled(self):
"""
This function is called in disabled mode. You should not
override this function; rather, you should override the
:meth:`disabledPeriodic` function instead.
.. warning:: Internal API, don't override
"""
self.__nt.putString('mode', 'disabled')
ds_attached = None
delay = PreciseDelay(self.control_loop_wait_time)
self._on_mode_disable_components()
try:
self.disabledInit()
except:
self.onException(forceReport=True)
while self.isDisabled():
if ds_attached != self.ds.isDSAttached():
ds_attached = not ds_attached
self.__nt.putBoolean('is_ds_attached', ds_attached)
try:
self.disabledPeriodic()
except:
self.onException()
delay.wait()
def operatorControl(self):
"""
This function is called in teleoperated mode. You should not
override this function; rather, you should override the
:meth:`teleopPeriodics` function instead.
.. warning:: Internal API, don't override
"""
self.__nt.putString('mode', 'teleop')
# don't need to update this during teleop -- presumably will switch
# modes when ds is no longer attached
self.__nt.putBoolean('is_ds_attached', self.ds.isDSAttached())
delay = PreciseDelay(self.control_loop_wait_time)
# initialize things
self._on_mode_enable_components()
try:
self.teleopInit()
except:
self.onException(forceReport=True)
while self.isOperatorControl() and self.isEnabled():
#self._update_autosend()
try:
self.teleopPeriodic()
except:
self.onException()
self._execute_components()
self._update_feedback()
self.robotPeriodic()
delay.wait()
self._on_mode_disable_components()
def operatorControl(self):
self.can_forklift.set_manual(0)
self.tote_forklift.set_manual(0)
self.tote_forklift.set_pos_stack5()
delay = PreciseDelay(self.control_loop_wait_time)
self.logger.info("Entering teleop mode")
while self.isOperatorControl() and self.isEnabled():
self.sensor.update()
#self.calibrator.calibrate()
self.drive.move(self.joystick1.getY(), self.joystick1.getX(),
self.joystick2.getX(), True)
#
# Can forklift controls
#
if self.joystick1.getRawButton(5):
self.can_forklift.set_manual(1)
elif self.joystick1.getRawButton(4):
self.can_forklift.set_manual(-1)
elif self.canUp.get():
self.can_forklift.raise_forklift()
elif self.canDown.get():
self.can_forklift.lower_forklift()
if self.canTop.get():
self.can_forklift.set_pos_top()
elif self.canBottom.get():
self.can_forklift.set_pos_bottom()
# #Tote forklift controls##
if self.joystick2.getRawButton(5):
self.tote_forklift.set_manual(1)
elif self.joystick2.getRawButton(4):
self.tote_forklift.set_manual(-1)
elif self.toteUp.get():
self.tote_forklift.raise_forklift()
elif self.toteDown.get():
self.tote_forklift.lower_forklift()
if self.toteTop.get():
self.tote_forklift.set_pos_top()
elif self.toteBottom.get():
self.tote_forklift.set_pos_bottom()
# INFRARED DRIVE#
if self.joystick2.getTrigger():
self.drive.isTheRobotBackwards = False
self.align.align()
elif self.autoLift:
if self.sensor.toteLimitL and self.sensor.toteLimitR:
self.tote_forklift.raise_forklift()
if self.joystick2.getRawButton(11):
self.drive.reset_gyro_angle()
if self.joystick2.getRawButton(8):
self.drive.wall_strafe(-.7)
elif self.joystick2.getRawButton(9):
self.drive.wall_strafe(.7)
# REVERSE DRIVE#
if self.reverseDirection.get():
self.drive.switch_direction()
if self.joystick1.getRawButton(10):
self.sweeper_relay.set(RelayValue.kForward)
elif self.joystick1.getRawButton(11):
self.sweeper_relay.set(RelayValue.kReverse)
else:
self.sweeper_relay.set(RelayValue.kOff)
if self.toteTo != self.oldTote:
if self.toteTo == 0:
self.tote_forklift._set_position(0)
elif self.toteTo == 1:
self.tote_forklift._set_position(1)
elif self.toteTo == 2:
self.tote_forklift._set_position(2)
elif self.toteTo == 3:
self.tote_forklift._set_position(3)
elif self.toteTo == 4:
self.tote_forklift._set_position(4)
elif self.toteTo == 2048:
self.tote_forklift.set_pos_top()
self.oldTote = self.toteTo
if self.canTo != self.oldCan:
if self.canTo == 0:
self.can_forklift._set_position(0)
elif self.canTo == 1:
self.can_forklift._set_position(1)
elif self.canTo == 2:
self.can_forklift._set_position(2)
elif self.canTo == 3:
self.can_forklift._set_position(3)
elif self.canTo == 4:
self.can_forklift._set_position(4)
elif self.canTo == 2048:
self.can_forklift.set_pos_top()
elif self.canTo == 7000:
self.can_forklift.set_pos_7000()
self.oldCan = self.canTo
if self.reverseRobot != self.oldReverseRobot:
if self.reverseRobot == 0:
self.drive.switch_direction()
self.oldReverseRobot = self.reverseRobot
self.ui_joystick_buttons()
self.smartdashbord_update()
self.update()
# Replaces wpilib.Timer.delay()
delay.wait()
