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 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()
self._update_feedback()
self.robotPeriodic()
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
"""
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():
try:
self.teleopPeriodic()
except:
self.onException()
self._execute_components()
delay.wait()
self._on_mode_disable_components()
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):
"""
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 disabled(self):
"""
This function is called in disabled mode. You should not
override this function; rather, you should override the
:meth:`disabledPeriodics` function instead.
.. warning:: Internal API, don't override
"""
delay = PreciseDelay(self.control_loop_wait_time)
self._on_mode_disable_components()
try:
self.disabledInit()
except:
self.onException(forceReport=True)
while self.isDisabled():
try:
self.disabledPeriodic()
except:
self.onException()
delay.wait()
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()
def operatorControl(self):
self.tote_forklift.set_manual(0)
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()
try:
self.drive.move(self.joystick1.getY()*((1+self.joystick1.getZ())/2), self.joystick1.getX(), self.joystick2.getX(),True)
except:
if not self.isFMSAttached():
raise
#
# Can forklift controls
#
## Tote forklift controls##
try:
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()
except:
if not self.isFMSAttached():
raise
if self.toteTo.value >= 0:
toteTo = int(self.toteTo.value)
if toteTo == 2048:
self.tote_forklift.set_pos_top()
else:
self.tote_forklift._set_position(toteTo)
self.sd.putDouble('toteTo', -1)
#
# Driver-enabled automatic alignment code
#
try:
if self.joystick2.getTrigger():
# self.drive.isTheRobotBackwards = False
# self.align.align()
#elif self.autoLift.value:
self.autoLifter.autolift()
except:
if not self.isFMSAttached():
raise
#
# Utilities
#
try:
if self.joystick2.getRawButton(11):
self.drive.reset_gyro_angle()
except:
if not self.isFMSAttached():
raise
try:
if self.joystick2.getRawButton(8):
self.drive.wall_strafe(-.7)
elif self.joystick2.getRawButton(9):
self.drive.wall_strafe(.7)
except:
if not self.isFMSAttached():
raise
#
# Reverse drive
#
try:
if self.reverseDirection.get():
self.drive.switch_direction()
except:
if not self.isFMSAttached():
raise
try:
if self.reverseRobot.value != self.oldReverseRobot:
if self.reverseRobot.value == 0:
self.drive.switch_direction()
self.oldReverseRobot = self.reverseRobot.value
except:
if not self.isFMSAttached():
raise
#
# At the end of autonomous mode, pick up the three bins in
# front of us if enabled
#
#if self.autoPickup.value:
# try:
# # End autopickup when the driver gives joystick input
# if abs(self.joystick1.getX())>.1 or abs(self.joystick1.getY())>.1 or abs(self.joystick2.getX())>.1:
# self.sd.putBoolean('autoPickup', False)
# else:
# if not self.sensor.is_against_tote():
# self.drive.move(self.autoPickupSpeed.value, 0, 0)
# else:
# # Move slow once we hit the totes
# self.drive.move(-0.1, 0, 0)
#
# self.autoLifter.autolift()
#
# except:
# self.sd.putBoolean('autoPickup', False)
# if not self.isFMSAttached():
# raise
try:
self.ui_joystick_buttons()
except:
if not self.isFMSAttached():
raise
try:
self.smartdashboard_update()
except:
if not self.isFMSAttached():
raise
try:
self.update()
except:
if not self.isFMSAttached():
raise
# Replaces wpilib.Timer.delay()
delay.wait()
