Esempio n. 1
0
def test_retain_wheel_direction():
    # When the joystick is returned to the centre, keep the last direction that the wheels were pointing
    chassis = Chassis()
    for name, module in chassis._modules.items():
        module.steer(math.pi / 4.0)
    chassis.drive(0.0, 0.0, 0.0)
    for name, module in chassis._modules.items():
        assert abs(constrain_angle(module.direction - math.pi / 4.0)) < epsilon
    # Should not matter what the throttle is, even if it is zero
    chassis.drive(0.0, 0.0, 0.0)
    for name, module in chassis._modules.items():
        assert abs(constrain_angle(module.direction - math.pi / 4.0)) < epsilon
Esempio n. 2
0
def test_absolute(robot):
    epsilon = 0.01  # Tolerance for angular floating point errors (~0.05 degrees)
    chassis = Chassis()
    reset_chassis(chassis)
    # Absolute argument should point the modules in the absolute direction
    # for diagnostic purposes
    chassis.drive(-1.0, -1.0, 0.0, absolute=True)
    for module in chassis._modules.values():
        assert abs(constrain_angle(module.direction + 3.0 / 4.0 * math.pi)) < epsilon
    reset_chassis(chassis)
Esempio n. 3
0
 def breach_defence(self):
     if self.chassis.distance_pid.onTarget():
         self.chassis.distance_pid.disable()
         # Let the distance PID do its magic...
         # Turn off the distance PID, and spin to the right angle
         self.chassis.heading_hold_pid.setSetpoint(constrain_angle(
                 self.chassis.heading_hold_pid.getSetpoint() +
                 self.delta_heading)
             )
         self.defeater_motor.set(0.3)
         self.next_state('spinning')
Esempio n. 4
0
 def on_iteration(self, tm):
     '''Drive forward the same amount, then move by delta_x and delta_y
     to the position where the vision and range finder take over.
     Final change in heading is specified too.'''
     if self.state == States.init:
         if self.portcullis:
             self.defeater_motor.set(-0.5)
         if not self.chassis.onTarget():
             return
         self.chassis.field_displace(self.straight, 0.0)
         self.state = States.through_obstacle
         self.logger.info("SETPOINT: " + str(self.chassis.distance_pid.getSetpoint()))
     if self.state == States.through_obstacle and self.chassis.distance_pid.onTarget():
         # Let the distance PID do its magic...
         # Turn off the distance PID, and spin to the right angle
         self.logger.info("Obstacle finished, distance: " + str(self.chassis.distance))
         self.chassis.distance_pid.disable()
         self.chassis.heading_hold_pid.setSetpoint(constrain_angle(self.chassis.heading_hold_pid.getSetpoint() + self.delta_heading))
         self.defeater_motor.set(0.3)
         self.state = States.spinning
     if self.state == States.spinning and self.chassis.heading_hold_pid.onTarget():
         # Turn on the distance PID for the next displacement
         self.chassis.field_displace(self.delta_x, self.delta_y)
         self.state = States.strafing
     if self.state == States.strafing and self.chassis.distance_pid.onTarget():
         # Dead reckoning is done - engage the rangefinder
         # Leave the distance PID running as it will read the rf for us
         self.logger.info("Strafing finished, distance: " + str(self.chassis.distance))
         self.state = States.range_finding
         self.chassis.distance_pid.setSetpoint(0.0)
         self.chassis.distance_pid.reset()
         self.chassis.range_setpoint = self.chassis.correct_range  # m
     if self.state == States.range_finding and self.chassis.on_range_target(): #self.chassis.distance_pid.onTarget():
         # Range is good, now turn on the vision tracking
         self.chassis.track_vision = True
         self.chassis.distance_pid.setSetpoint(0.0)
         self.chassis.distance_pid.reset()
         self.state = States.goal_tracking
         self.logger.info("On range, distance: " + str(self.chassis.distance))
         self.shooter.change_state(shooter.States.shooting)
         self.shooting = True
     if (self.state == States.goal_tracking and self.chassis.on_vision_target()) or ((time.time() - self.start_time) > 12): #self.chassis.distance_pid.onTarget():
         # We made it to the target point, so fire away!
         self.shooter.change_state(shooter.States.shooting)
         self.intake.state = intake.States.fire
         self.state = States.firing
         self.chassis.range_setpoint = 0.0
         self.chassis.track_vision = False
     if self.state == States.firing and self.shooter.state == shooter.States.shooting:
         self.shooter.change_state(shooter.States.shooting)
         self.intake.state = intake.States.fire
         self.chassis.field_oriented = True
Esempio n. 5
0
def test_chassis(robot, wpilib):
    epsilon = 0.01  # Tolerance for angular floating point errors (~0.05 degrees)
    chassis = Chassis()

    # vX is out the left side of the robot, vY is out of the front, vZ is upwards, so a +ve rotation is counter-clockwise
    #             vX   vY   vZ   throttle
    reset_chassis(chassis)
    # test x axis
    chassis.drive(1.0, 0.0, 0.0, 1.0)
    for name, module in chassis._modules.items():
        assert abs(constrain_angle(module.direction)) % math.pi  < epsilon
    reset_chassis(chassis)

    # test y axis
    chassis.drive(0.0, 1.0, 0.0)
    for name, module in chassis._modules.items():
        # test weather each module is facing in the right direction
        assert abs(constrain_angle(math.pi / 2.0 - module.direction)) < epsilon
    reset_chassis(chassis)

    vz = {'a': math.atan2(-Chassis.length, Chassis.width),  # the angle that module a will go to if we spin on spot
          'b': math.atan2(Chassis.length, Chassis.width),
          'c': math.atan2(-Chassis.length, Chassis.width),
          'd': math.atan2(Chassis.length, Chassis.width)
          }

    chassis.drive(0.0, 0.0, 1.0)

    for name, module in chassis._modules.items():
        assert abs(constrain_angle(module.direction - vz[name])) < epsilon
    reset_chassis(chassis)

    chassis.drive(1.0, 1.0, 0.0)
    for module in chassis._modules.values():
        assert abs(constrain_angle(module.direction - math.pi / 4.0)) < epsilon
    reset_chassis(chassis)
Esempio n. 6
0
    def teleopPeriodic(self):
        """This function is called periodically during operator control."""

        try:
            if self.debounce(6, gamepad=True):
                self.boulder_automation.toggle_shoot_boulder()
        except:
            self.onException()

        try:
            if self.debounce(2) or self.debounce(1, gamepad=True):
                self.boulder_automation.toggle_intake_boulder()
        except:
            self.onException()

        try:
            if self.debounce(7):
                self.chassis.toggle_field_oriented()
        except:
            self.onException()

        try:
            if self.debounce(8):
                enabled = self.heading_hold_pid.isEnable()
                self.heading_hold_pid.disable()
                self.bno055.resetHeading()
                self.heading_hold_pid.setSetpoint(
                    constrain_angle(self.bno055.getAngle()))
                self.heading_hold_pid.reset()
                if enabled:
                    self.heading_hold_pid.enable()
        except:
            self.onException()
        """try:
            if self.debounce(10):
                self.chassis.toggle_vision_tracking()
        except:
            self.onException()"""

        try:
            if self.debounce(10):
                self.chassis.toggle_range_holding(self.chassis.correct_range)
        except:
            self.onException()

        try:
            if self.debounce(1) or self.debounce(8, gamepad=True):
                self.boulder_automation.toggle_shoot_boulder()
        except:
            self.onException()

        try:
            if self.debounce(9):
                self.chassis.toggle_heading_hold()
        except:
            self.onException()

        try:
            if self.debounce(4):
                self.defeater.up()
        except:
            self.onException()

        try:
            if self.debounce(5):
                self.shooter.stop()
                self.intake.stop()
        except:
            self.onException()

        try:
            if self.debounce(3):

                self.chassis.track_vision = True
                self.chassis.range_setpoint = self.chassis.correct_range
                self.chassis.distance_pid.enable()
                # self.shooter.start_shoot()
        except:
            self.onException()

        try:
            if self.debounce(6):
                self.defeater.down()
        except:
            self.onException()
        """try:
            if self.debounce(10):
                self.shooter.backdrive()
                self.intake.backdrive()
        except:
            self.onException()"""

        try:
            if self.joystick.getPOV() != -1:
                self.chassis.heading_hold = True
                direction = 0.0
                if self.joystick.getPOV() == 0:
                    # shooter centre goal
                    direction = math.pi
                elif self.joystick.getPOV() == 90:
                    # shooter right goal
                    direction = math.pi / 3.0 + math.pi
                elif self.joystick.getPOV() == 270:
                    # shooter left goal
                    direction = -math.pi / 3.0 + math.pi
                elif self.joystick.getPOV() == 180:
                    direction = 0.0
                self.chassis.set_heading_setpoint(direction)
        except:
            self.onException()

        try:
            if self.joystick.getRawButton(11) or self.gamepad.getRawButton(2):
                self.chassis.field_oriented = False
            else:
                self.chassis.field_oriented = True
        except:
            self.onException()

        try:
            if self.gamepad.getRawButton(3):
                self.boulder_automation.engage("backdrive_manual")
            elif self.boulder_automation.current_state == "backdrive_manual":
                self.boulder_automation.done()
        except:
            self.onException()
        """try:
            if self.debounce(1, gamepad=True):
                self.chassis.zero_encoders()
                self.chassis.distance_pid.setSetpoint(1.2)
                self.chassis.distance_pid.enable()
        except:
            self.onException()"""

        try:
            if self.debounce(10, gamepad=True):
                self.vision.write_image()
        except:
            self.onException()

        try:
            if self.joystick.getRawButton(12):
                self.joystick_rate = 0.6
            else:
                self.joystick_rate = 0.4
        except:
            self.onException()

        self.chassis.inputs = [
            -rescale_js(self.joystick.getY(), deadzone=0.05, exponential=1.2),
            -rescale_js(self.joystick.getX(), deadzone=0.05, exponential=1.2),
            -rescale_js(self.joystick.getZ(),
                        deadzone=0.2,
                        exponential=15.0,
                        rate=self.joystick_rate),
            (self.joystick.getThrottle() - 1.0) / -2.0
        ]
        for input in self.chassis.inputs[0:3]:
            if input != 0.0:
                # Break out of auto if we move the stick
                self.chassis.distance_pid.disable()
                self.chassis.range_setpoint = None
                self.chassis.track_vision = False
                #self.chassis.field_oriented = True
        self.putData()
Esempio n. 7
0
 def on_iteration(self, tm):
     '''Drive forward the same amount, then move by delta_x and delta_y
     to the position where the vision and range finder take over.
     Final change in heading is specified too.'''
     rf = self.chassis.range_finder.pidGet()
     #self.logger.info("VISION OUTPUT: " + str(self.chassis.vision.pidGet()) + " COUNTER: " + str(self.chassis.vision.no_vision_counter))
     if self.state == States.init:
         if self.portcullis:
             self.defeater_motor.set(-0.5)
         if not self.chassis.onTarget():
             return
         self.chassis.field_displace(self.straight, 0.0)
         self.state = States.through_obstacle
         self.chassis.distance_pid.setOutputRange(-0.55, 0.55)
         self.logger.info("SETPOINT: " + str(self.chassis.distance_pid.getSetpoint()))
     if self.state == States.through_obstacle and self.chassis.distance_pid.onTarget():
         # Let the distance PID do its magic...
         # Turn off the distance PID, and spin to the right angle
         self.logger.info("Obstacle finished, distance: " + str(self.chassis.distance))
         self.chassis.distance_pid.disable()
         self.chassis.heading_hold_pid.setSetpoint(constrain_angle(self.chassis.heading_hold_pid.getSetpoint() + self.delta_heading))
         self.defeater_motor.set(0.3)
         self.state = States.spinning
     if self.state == States.spinning and self.chassis.heading_hold_pid.onTarget():
         # Turn on the distance PID for the next displacement
         self.chassis.field_displace(self.delta_x, self.delta_y)
         self.state = States.strafing
     if self.state == States.strafing and self.chassis.distance_pid.onTarget():
         # Dead reckoning is done - engage the rangefinder
         # Leave the distance PID running as it will read the rf for us
         self.chassis.distance_pid.setOutputRange(-0.4, 0.4)
         self.logger.info("Strafing finished, distance: " + str(self.chassis.distance))
         self.state = States.range_finding
         self.chassis.distance_pid.reset()
         self.chassis.zero_encoders()
         self.chassis.range_setpoint = self.chassis.correct_range  # m
         self.chassis.distance_pid.reset()
         self.chassis.distance_pid.enable()
         self.logger.info(self.chassis.distance_pid.onTarget())
         #TODO: GET RID OF THIS STUFF IF YOU WANT TO RANGE
         """self.shooter.change_state(shooter.States.shooting)
         self.intake.state = intake.States.fire
         self.state = States.firing
         self.chassis.distance_pid.reset()
         self.chassis.distance_pid.setSetpoint(0.0)
         self.chassis.range_setpoint = 0.0
         self.chassis.track_vision = False"""
     if self.state == States.range_finding and self.chassis.on_range_target(): #self.chassis.distance_pid.onTarget():
         # Range is good, now turn on the vision tracking
         self.chassis.track_vision = True
         self.chassis.distance_pid.reset()
         self.chassis.distance_pid.setSetpoint(0.0)
         self.chassis.zero_encoders()
         self.chassis.distance_pid.reset()
         self.chassis.distance_pid.enable()
         self.state = States.goal_tracking
         self.logger.info("On range, distance: " + str(self.chassis.distance))
         self.boulder_automation.disarm()
         self.boulder_automation.shoot_boulder()
         #TODO: GET RID OF THIS STUFF IF YOU WANT TO VISION 
         """self.shooter.change_state(shooter.States.shooting)
         self.intake.state = intake.States.fire
         self.state = States.firing
         self.chassis.distance_pid.reset()
         self.chassis.distance_pid.setSetpoint(0.0)
         self.chassis.range_setpoint = 0.0
         self.chassis.track_vision = False"""
     if (self.state == States.goal_tracking and self.chassis.on_vision_target()) and self.chassis.on_range_target():# or ((time.time() - self.start_time) > 12): #self.chassis.distance_pid.onTarget():
         # We made it to the target point, so fire away!
         self.boulder_automation.arm()
         self.boulder_automation.shoot_boulder()
         self.state = States.firing
         self.chassis.range_setpoint = 0.0
         self.chassis.track_vision = False
         self.chassis.distance_pid.reset()
     if self.state == States.firing and self.shooter.state == shooter.States.shooting:
         self.boulder_automation.arm()
         self.boulder_automation.shoot_boulder()
         self.chassis.field_oriented = True
Esempio n. 8
0
    def teleopPeriodic(self):
        """This function is called periodically during operator control."""
        try:
            if self.debounce(2) or self.debounce(1, gamepad=True):
                self.intake.toggle()
        except:
            self.onException()

        try:
            if self.debounce(7):
                self.chassis.toggle_field_oriented()
        except:
            self.onException()

        try:
            if self.debounce(8):
                enabled = self.heading_hold_pid.isEnable()
                self.heading_hold_pid.disable()
                self.bno055.resetHeading()
                self.heading_hold_pid.setSetpoint(constrain_angle(self.bno055.getAngle()))
                self.heading_hold_pid.reset()
                if enabled:
                    self.heading_hold_pid.enable()
        except:
            self.onException()

        try:
            if self.debounce(10):
                self.chassis.toggle_vision_tracking()
        except:
            self.onException()

        try:
            if self.debounce(12):
                self.chassis.toggle_range_holding(self.chassis.correct_range)
        except:
            self.onException()

        try:
            if self.debounce(1):
                if self.shooter.state == shooter.States.off:
                    self.shooter.start_shoot()
                else:
                    self.shooter.toggle()
                self.intake.fire()
        except:
            self.onException()

        try:
            if self.debounce(9):
                self.chassis.toggle_heading_hold()
        except:
            self.onException()

        try:
            if self.debounce(4):
                self.defeater.up()
        except:
            self.onException()

        try:
            if self.debounce(5):
                self.shooter.stop()
                self.intake.stop()
        except:
            self.onException()

        try:
            if self.debounce(3):

                self.chassis.track_vision = True
                self.chassis.range_setpoint = self.chassis.correct_range
                self.chassis.distance_pid.enable()
                # self.shooter.start_shoot()
        except:
            self.onException()

        try:
            if self.debounce(6):
                self.defeater.down()
        except:
            self.onException()

        """try:
            if self.debounce(10):
                self.shooter.backdrive()
                self.intake.backdrive()
        except:
            self.onException()"""

        try:
            if self.joystick.getPOV() != -1:
                self.chassis.heading_hold = True
                direction = 0.0
                if self.joystick.getPOV() == 0:
                    # shooter centre goal
                    direction = math.pi
                elif self.joystick.getPOV() == 90:
                    # shooter right goal
                    direction = math.pi/3.0+math.pi
                elif self.joystick.getPOV() == 270:
                    # shooter left goal
                    direction = -math.pi/3.0+math.pi
                elif self.joystick.getPOV() == 180:
                    direction = 0.0
                self.chassis.set_heading_setpoint(direction)
        except:
            self.onException()

        try:
            if self.joystick.getRawButton(11) or self.gamepad.getRawButton(2):
                self.chassis.field_oriented = False 
            else:
                self.chassis.field_oriented = True
        except:
            self.onException()

        try:
            if self.gamepad.getRawButton(3):
                self.intake.backdrive_slow()
        except:
            self.onException()

        try:
            if self.debounce(1, gamepad=True):
                self.chassis.zero_encoders()
                self.chassis.distance_pid.setSetpoint(1.2)
                self.chassis.distance_pid.enable()
        except:
            self.onException()

        try:
            if self.gamepad.getRawButton(4):
                self.shooter.backdrive_recovery()
        except:
            self.onException()

        self.chassis.inputs = [-rescale_js(self.joystick.getY(), deadzone=0.05, exponential=1.2),
                            - rescale_js(self.joystick.getX(), deadzone=0.05, exponential=1.2),
                            - rescale_js(self.joystick.getZ(), deadzone=0.2, exponential=15.0, rate=0.3),
                            (self.joystick.getThrottle() - 1.0) / -2.0
                            ]
        for input in self.chassis.inputs[0:3]:
            if input != 0.0:
                # Break out of auto if we move the stick
                self.chassis.distance_pid.disable()
                self.chassis.range_setpoint = None
                self.chassis.track_vision = False
                self.chassis.field_oriented = True
        self.putData()
Esempio n. 9
0
 def on_iteration(self, tm):
     '''Drive forward the same amount, then move by delta_x and delta_y
     to the position where the vision and range finder take over.
     Final change in heading is specified too.'''
     rf = self.chassis.range_finder.pidGet()
     #self.logger.info("VISION OUTPUT: " + str(self.chassis.vision.pidGet()) + " COUNTER: " + str(self.chassis.vision.no_vision_counter))
     if self.state == States.init:
         if self.portcullis:
             self.defeater_motor.set(-0.5)
         if not self.chassis.onTarget():
             return
         self.chassis.field_displace(self.straight, 0.0)
         self.state = States.through_obstacle
         self.chassis.distance_pid.setOutputRange(-0.55, 0.55)
         self.logger.info("SETPOINT: " +
                          str(self.chassis.distance_pid.getSetpoint()))
     if self.state == States.through_obstacle and self.chassis.distance_pid.onTarget(
     ):
         # Let the distance PID do its magic...
         # Turn off the distance PID, and spin to the right angle
         self.logger.info("Obstacle finished, distance: " +
                          str(self.chassis.distance))
         self.chassis.distance_pid.disable()
         self.chassis.heading_hold_pid.setSetpoint(
             constrain_angle(self.chassis.heading_hold_pid.getSetpoint() +
                             self.delta_heading))
         self.defeater_motor.set(0.3)
         self.state = States.spinning
     if self.state == States.spinning and self.chassis.heading_hold_pid.onTarget(
     ):
         # Turn on the distance PID for the next displacement
         self.chassis.field_displace(self.delta_x, self.delta_y)
         self.state = States.strafing
     if self.state == States.strafing and self.chassis.distance_pid.onTarget(
     ):
         # Dead reckoning is done - engage the rangefinder
         # Leave the distance PID running as it will read the rf for us
         self.chassis.distance_pid.setOutputRange(-0.4, 0.4)
         self.logger.info("Strafing finished, distance: " +
                          str(self.chassis.distance))
         self.state = States.range_finding
         self.chassis.distance_pid.reset()
         self.chassis.zero_encoders()
         self.chassis.range_setpoint = self.chassis.correct_range  # m
         self.chassis.distance_pid.reset()
         self.chassis.distance_pid.enable()
         self.logger.info(self.chassis.distance_pid.onTarget())
         #TODO: GET RID OF THIS STUFF IF YOU WANT TO RANGE
         """self.shooter.change_state(shooter.States.shooting)
         self.intake.state = intake.States.fire
         self.state = States.firing
         self.chassis.distance_pid.reset()
         self.chassis.distance_pid.setSetpoint(0.0)
         self.chassis.range_setpoint = 0.0
         self.chassis.track_vision = False"""
     if self.state == States.range_finding and self.chassis.on_range_target(
     ):  #self.chassis.distance_pid.onTarget():
         # Range is good, now turn on the vision tracking
         self.chassis.track_vision = True
         self.chassis.distance_pid.reset()
         self.chassis.distance_pid.setSetpoint(0.0)
         self.chassis.zero_encoders()
         self.chassis.distance_pid.reset()
         self.chassis.distance_pid.enable()
         self.state = States.goal_tracking
         self.logger.info("On range, distance: " +
                          str(self.chassis.distance))
         self.boulder_automation.disarm()
         self.boulder_automation.shoot_boulder()
         #TODO: GET RID OF THIS STUFF IF YOU WANT TO VISION
         """self.shooter.change_state(shooter.States.shooting)
         self.intake.state = intake.States.fire
         self.state = States.firing
         self.chassis.distance_pid.reset()
         self.chassis.distance_pid.setSetpoint(0.0)
         self.chassis.range_setpoint = 0.0
         self.chassis.track_vision = False"""
     if (
             self.state == States.goal_tracking
             and self.chassis.on_vision_target()
     ) and self.chassis.on_range_target(
     ):  # or ((time.time() - self.start_time) > 12): #self.chassis.distance_pid.onTarget():
         # We made it to the target point, so fire away!
         self.boulder_automation.arm()
         self.boulder_automation.shoot_boulder()
         self.state = States.firing
         self.chassis.range_setpoint = 0.0
         self.chassis.track_vision = False
         self.chassis.distance_pid.reset()
     if self.state == States.firing and self.shooter.state == shooter.States.shooting:
         self.boulder_automation.arm()
         self.boulder_automation.shoot_boulder()
         self.chassis.field_oriented = True