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