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