Ejemplo n.º 1
0
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)
Ejemplo n.º 2
0
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)
Ejemplo n.º 4
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
Ejemplo n.º 5
0
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()