class PreciseDelay(object):
"""
Used to synchronize a timing loop.
Usage:
delay = PreciseDelay(time_to_delay)
while something:
# do things here
delay.wait()
"""
def __init__(self, delay_period):
""" :param delay_period: The amount of time to do a delay """
self.timer = Timer()
self.delay_period = delay_period
self.timer.start()
def wait(self):
""" Waits until the delay period has passed """
# we must *always* yield here, so other things can run
Timer.delay(0.001)
while not self.timer.hasPeriodPassed(self.delay_period):
Timer.delay(0.001)
class MoveElevatorToPosition(Command):
def __init__(self, position):
super().__init__('MoveElevatorDown')
self._elevator = self.getRobot().elevator
self.requires(self._elevator)
self._logger = self.getRobot().logger
self._speed = robotmap.ElevatorSpeed
self._smartDashboard = NetworkTables.getTable('SmartDashboard')
self._targetPosition = position
self._timer = Timer()
def initialize(self):
self._elevator.stop()
self._timer.reset()
self._timer.start()
self._smartDashboard.putString("ElevatorPosition",
str(self._elevator.currentPosition()))
if self._elevator.currentPosition() > self._targetPosition.value:
self._speed = -self._speed
if self._targetPosition == ElevatorPosition.INITIAL_DEPLOY:
while not self._timer.hasPeriodPassed(robotmap.Deploy_Delay):
continue
def execute(self):
self._elevator.move(self._speed)
self._smartDashboard.putString("ElevatorPosition",
str(self._elevator.currentPosition()))
def isFinished(self):
if self._elevator.currentPosition() == self._targetPosition.value:
return True
elif self._elevator.atLimit(
ElevatorLimit.LOWER) or self._elevator.atLimit(
ElevatorLimit.UPPER):
return True
else:
return False
def interrupted(self):
self.end()
def end(self):
self._elevator.stop()
class FollowJoystick(Command):
"""
This command will read the joysticks' y-axes and uses tank drive.
"""
def __init__(self):
super().__init__('Follow Joystick')
self.requires(subsystems.motors)
self.logger = logging.getLogger("robot")
self.drive = RectifiedDrive(25, 0.05, squared_inputs=True)
self.sd = NetworkTables.getTable("SmartDashboard")
self.timer = Timer()
self.timer.start()
def execute(self):
if self.timer.hasPeriodPassed(0.05): # period of 0.05 seconds
# tank drive
# subsystems.motors.robot_drive.tankDrive(oi.joystick, robotmap.joystick.left_port, oi.joystick,
# robotmap.joystick.right_port, True)
# arcade drive
# subsystems.motors.robot_drive.arcadeDrive(oi.joystick)
# rectified arcade drive
power = -oi.joystick.getRawAxis(
robotmap.joystick.forwardAxis) * 0.75
angular_vel = oi.joystick.getRawAxis(
robotmap.joystick.steeringAxis)
if self.sd.containsKey("slowmode") and self.sd.getBoolean(
"slowmode"):
power *= 0.75
angular_vel *= 0.75
if abs(angular_vel) < 0.1: # tolerance
angular_vel = 0
self.drive.rectified_drive(power, angular_vel)
def end(self):
subsystems.motors.robot_drive.setLeftRightMotorOutputs(0, 0)
class Tachyon(SampleRobot):
# because robotInit is called straight from __init__
def robotInit(self):
hardware.init() # this makes everything not break
self.drive = drive.Drive()
self.pneumatics = pneumatics.Pneumatics()
self.intake = intake.Intake()
self.elevator = elevator.Elevator()
self.components = {
'drive': self.drive,
'pneumatics': self.pneumatics,
'intake': self.intake,
'elevator': self.elevator,
}
self.nt_timer = Timer(
) # timer for SmartDashboard update so we don't use all our bandwidth
self.nt_timer.start()
self.autonomous_modes = AutonomousModeSelector('autonomous',
self.components)
self.state = States.STACKING
def autonomous(self):
self.autonomous_modes.run(CONTROL_LOOP_WAIT_TIME,
iter_fn=self.update_all)
Timer.delay(CONTROL_LOOP_WAIT_TIME)
def update_all(self):
self.update()
def disabled(self):
while self.isDisabled():
Timer.delay(0.01)
def operatorControl(self):
precise_delay = delay.PreciseDelay(CONTROL_LOOP_WAIT_TIME)
while self.isOperatorControl() and self.isEnabled():
# States!
if hardware.driver.left_trigger():
self.state = States.DROPPING
elif hardware.operator.right_trigger():
self.state = States.CAPPING
else:
self.state = States.STACKING
if self.elevator.is_full():
self.intake.pause()
elif not self.elevator.has_bin and not self.elevator.tote_first:
self.intake.intake_bin()
else:
self.intake.intake_tote()
if hardware.driver.right_bumper():
self.intake.open()
else:
self.intake.close()
if self.state == States.STACKING:
if hardware.operator.left_bumper() or True:
self.elevator.stack_tote_first()
if hardware.driver.a():
self.elevator.manual_stack()
elif self.state == States.DROPPING:
self.elevator.drop_stack()
self.elevator.reset_stack()
self.intake.pause()
self.intake.open()
if hardware.driver.right_bumper():
self.intake.close()
elif self.state == States.CAPPING:
self.elevator.cap()
wheel = deadband(hardware.driver.right_x(), .2)
throttle = -deadband(hardware.driver.left_y(), .2)
quickturn = hardware.driver.left_bumper()
low_gear = hardware.driver.right_trigger()
self.drive.cheesy_drive(wheel, throttle, quickturn, low_gear)
driver_dpad = hardware.driver.dpad()
if driver_dpad == 180: # down on the dpad
self.drive.set_distance_goal(-2)
elif driver_dpad == 0:
self.drive.set_distance_goal(2)
elif driver_dpad == 90:
self.drive.set_distance_goal(-18)
operator_dpad = hardware.operator.dpad(
) # You can only call it once per loop, bcus dbouncing
if operator_dpad == 0 and self.elevator.tote_count < 6:
self.elevator.tote_count += 1
elif operator_dpad == 180 and self.elevator.tote_count > 0:
self.elevator.tote_count -= 1
elif operator_dpad == 90:
self.elevator.has_bin = not self.elevator.has_bin
if hardware.operator.start():
self.elevator.reset_stack()
if hardware.operator.b():
self.intake.set(0) # Pause?!
if hardware.operator.a() or hardware.driver.b():
self.intake.set(-1)
# Deadman switch. very important for safety (at competitions).
if not self.ds.isFMSAttached(
) and not hardware.operator.left_trigger(
) and False: # TODO re-enable at competitions
for component in self.components.values():
component.stop()
else:
self.update()
precise_delay.wait()
def test(self):
while self.isTest() and self.isEnabled():
LiveWindow.run()
for component in self.components.values():
component.stop()
if self.nt_timer.hasPeriodPassed(.5):
component.update_nt()
def update(self):
""" Calls the update functions for every component """
for component in self.components.values():
try:
component.update()
if self.nt_timer.hasPeriodPassed(.5):
component.update_nt()
except Exception as e:
if self.ds.isFMSAttached():
log.error("In subsystem %s: %s" % (component, e))
else:
raise
class Tachyon(SampleRobot):
# noinspection PyAttributeOutsideInit
# because robotInit is called straight from __init__
def robotInit(self):
self.chandler = XboxController(0)
self.meet = XboxController(1)
self.drive = drive.Drive() # So redundant omg
self.pneumatics = pneumatics.Pneumatics()
self.intake = intake.Intake()
self.elevator = elevator.Elevator()
self.components = {
'drive': self.drive,
'pneumatics': self.pneumatics,
'intake': self.intake,
'elevator': self.elevator,
}
self.nt_timer = Timer() # timer for SmartDashboard update so we don't use all our bandwidth
self.nt_timer.start()
self.autonomous_modes = AutonomousModeSelector('autonomous', self.components)
quickdebug.add_printables(self, ('match_time', Timer.getMatchTime))
quickdebug.init()
def autonomous(self):
self.autonomous_modes.run(CONTROL_LOOP_WAIT_TIME, iter_fn=self.update_all)
Timer.delay(CONTROL_LOOP_WAIT_TIME)
def update_all(self):
self.update()
self.update_networktables()
def disabled(self):
while self.isDisabled():
self.update_networktables()
Timer.delay(0.01)
def operatorControl(self):
precise_delay = delay.PreciseDelay(CONTROL_LOOP_WAIT_TIME)
while self.isOperatorControl() and self.isEnabled():
if self.meet.left_bumper() or self.elevator.has_bin:
self.elevator.stack_tote_first()
if self.elevator.full():
self.intake.spin(0)
else:
self.intake.intake_tote()
else:
self.intake.intake_bin()
self.elevator.force_stack = self.chandler.a()
if self.chandler.right_bumper():
self.intake.open()
else:
self.intake.close()
if self.chandler.left_trigger(): # If we're trying to drop the stack
self.intake.spin(0)
self.intake.open()
self.elevator.drop_stack()
wheel = deadband(self.chandler.right_x(), .2)
throttle = -deadband(self.chandler.left_y(), .2) * .8
if self.chandler.right_trigger():
wheel *= 0.3
throttle *= 0.3
self.drive.cheesy_drive(wheel, throttle, self.chandler.left_bumper())
self.drive.auto_drive() # encoders n shit
ticks = self.chandler.dpad()
if ticks == 180: # dowdn on the dpad
self.drive.set_distance_goal(-2)
elif ticks == 0:
self.drive.set_distance_goal(2)
elif ticks == 90:
self.drive.set_distance_goal(-18)
dpad = self.meet.dpad() # You can only call it once per loop, bcus dbouncing
if dpad == 0 and self.elevator.tote_count < 6:
self.elevator.add_tote()
elif dpad == 180 and self.elevator.tote_count > 0:
self.elevator.remove_tote()
elif dpad == 90:
self.elevator.set_bin(not self.elevator.has_bin)
if self.meet.start():
self.elevator._new_stack = True
if self.meet.b():
self.intake.spin(0)
if self.meet.a() or self.chandler.b():
self.intake.spin(-1)
self.elevator.auto_stacking = not self.meet.right_bumper() # Disable automatic stacking if bumper pressed
# Deadman's switch! very important for safety.
if not self.ds.isFMSAttached() and not self.meet.left_trigger():
for component in self.components.values():
component.stop()
else:
self.update()
self.update_networktables()
precise_delay.wait()
def test(self):
for component in self.components.values():
component.stop()
while self.isTest() and self.isEnabled():
LiveWindow.run()
self.update_networktables()
def update(self):
""" Calls the update functions for every component """
for component in self.components.values():
try:
component.update()
except Exception as e:
if self.ds.isFMSAttached():
log.error("In subsystem %s: %s" % (component, e))
else:
raise e
def update_networktables(self):
if not self.nt_timer.hasPeriodPassed(0.2): # we don't need to update every cycle
return
quickdebug.sync()
