def deliver_left_coupler():
g.turn_left_gyro(130)
g.forwards_gyro_until_bump()
m.move_wrist(884)
s.wfollow_left_until_black_right_front(0)
s.wfollow_left_until_black_left()
g.backwards_gyro_until_both_white_cliffs()
s.align_close_cliffs()
g.backwards_gyro(1150)
g.turn_right_gyro(25)
m.move_arm(c.ARM_JUST_BARELY_ON_T_POS)
def align_on_wall_right():
m.base_veer_right(0.5)
wait_until(isRoombaBumped)
m.deactivate_motors()
u.halve_speeds()
m.base_turn_left()
wait_until(isRoombaNotBumped)
m.deactivate_motors()
msleep(100)
g.turn_right_gyro(4)
msleep(100)
def get_firefighters():
if c.SAFE_HOSPITAL == c.NEAR_ZONE:
g.drive_gyro_until_black_left()
m.lift_ambulance_arm()
g.drive_gyro_through_line_left()
else:
g.drive_gyro_until_black_left()
m.lift_ambulance_arm()
g.drive_gyro_through_line_left()
s.lfollow_right_pid_until_black_left(bias=15)
s.turn_right_until_black(0)
s.turn_right_until_white()
m.lower_arm()
g.drive_gyro_through_line_left()
s.turn_right_until_black_left()
s.turn_right_until_white_left()
g.turn_right_gyro(15)
def get_left_coupler():
# Gets to the bottom left square
g.forwards_gyro_through_line_rfcliff()
s.align_far_fcliffs()
g.forwards_gyro_until_white_rfcliff()
s.align_on_wall_left()
s.wfollow_left_smooth_until_white_rfcliff(0)
s.wfollow_left_smooth_until_black_rfcliff()
s.align_close_fcliffs()
# Turns right and goes to top left coupler
g.turn_right_gyro(85)
g.forwards_gyro_through_line_rfcliff()
s.lfollow_lfcliff_until_bump_pid(bias=15)
g.backwards_gyro(100)
s.turn_right_until_rfcliff_senses_white(0)
msleep(1500)
s.turn_right_until_rfcliff_senses_black(c.RIGHT_TURN_TIME * 6)
s.lfollow_rfcliff_until_lfcliff_senses_black_pid()
m.lower_arm()
g.turn_left_gyro(23)
g.backwards_gyro(1125)
#g.backwards_gyro_until_item_is_in_claw(1100)
pick_up_coupler()
def get_blocks():
g.backwards_gyro_until_black_left()
s.align_far()
g.backwards_gyro_through_line_left(0)
g.backwards_gyro_until_black_left()
s.align_far()
g.backwards_gyro_through_line_left(0)
g.backwards_gyro(300)
g.turn_right_gyro(90)
g.drive_gyro(2000, should_stop=False)
g.drive_gyro_until_white_left(0)
g.drive_gyro_until_black_left(0)
g.drive_gyro(500, should_stop=False)
g.drive_gyro_until_white_left(0)
g.drive_gyro(950)
s.turn_left_until_black()
m.open_claw()
m.lower_arm()
#s.lfollow_left_pid(13000, bias=15, should_stop=False)
s.lfollow_left_pid_until_black_third(time=21000, bias=10)
m.close_claw()
g.backwards_gyro(800)
m.lift_arm(1, 1)
s.lfollow_left_pid_until_black_right(bias=10)
def test_gyro_turns():
print "Turning right 90 degrees."
g.turn_right_gyro()
msleep(500)
print "Turning left 90 degrees."
g.turn_left_gyro()
msleep(500)
print "Turning right 180 degrees."
g.turn_right_gyro(180)
msleep(500)
print "Turning left 180 degrees."
g.turn_left_gyro(180)
msleep(500)
print "Turning right 360 degrees."
g.turn_right_gyro(360)
msleep(500)
print "Turning left 360 degrees."
g.turn_left_gyro(360)
msleep(500)
sd()
def calibrate_with_gyro_angle_calibration():
# Initialize variables.
c.MIN_SENSOR_VALUE_LCLIFF = 90000
c.MAX_SENSOR_VALUE_LCLIFF = 0
c.MAX_SENSOR_VALUE_RCLIFF = 0
c.MIN_SENSOR_VALUE_RCLIFF = 90000
c.MAX_SENSOR_VALUE_LFCLIFF = 0
c.MIN_SENSOR_VALUE_LFCLIFF = 90000
c.MAX_SENSOR_VALUE_RFCLIFF = 0
c.MIN_SENSOR_VALUE_RFCLIFF = 90000
angle = 0
error = 0
total_left_speed = 0
total_right_speed = 0
run_throughs = 0
sec = seconds() + 3
print "Running calibrate()"
m.activate_motors(int(c.BASE_LM_POWER / 2), int(c.BASE_RM_POWER / 2))
while seconds() < sec:
if get_create_lcliff_amt() > c.MAX_SENSOR_VALUE_LCLIFF:
c.MAX_SENSOR_VALUE_LCLIFF = get_create_lcliff_amt()
if get_create_lcliff_amt() < c.MIN_SENSOR_VALUE_LCLIFF:
c.MIN_SENSOR_VALUE_LCLIFF = get_create_lcliff_amt()
if get_create_rcliff_amt() > c.MAX_SENSOR_VALUE_RCLIFF:
c.MAX_SENSOR_VALUE_RCLIFF = get_create_rcliff_amt()
if get_create_rcliff_amt() < c.MIN_SENSOR_VALUE_RCLIFF:
c.MIN_SENSOR_VALUE_RCLIFF = get_create_rcliff_amt()
if get_create_lfcliff_amt() > c.MAX_SENSOR_VALUE_LFCLIFF:
c.MAX_SENSOR_VALUE_LFCLIFF = get_create_lfcliff_amt()
if get_create_lfcliff_amt() < c.MIN_SENSOR_VALUE_LFCLIFF:
c.MIN_SENSOR_VALUE_LFCLIFF = get_create_lfcliff_amt()
if get_create_rfcliff_amt() > c.MAX_SENSOR_VALUE_RFCLIFF:
c.MAX_SENSOR_VALUE_RFCLIFF = get_create_rfcliff_amt()
if get_create_rfcliff_amt() < c.MIN_SENSOR_VALUE_RFCLIFF:
c.MIN_SENSOR_VALUE_RFCLIFF = get_create_rfcliff_amt()
left_speed = int(c.BASE_LM_POWER / 2) + error
right_speed = int(c.BASE_RM_POWER / 2) - error
m.activate_motors(left_speed, right_speed)
total_left_speed += left_speed
total_right_speed += right_speed
run_throughs += 1
msleep(10)
angle += (gyro_z() - g.bias) * 10
error = 0.003830106222 * angle # Positive error means veering left. Negative means veering right.
m.deactivate_motors()
c.LCLIFF_BW = (
(c.MAX_SENSOR_VALUE_LCLIFF + c.MIN_SENSOR_VALUE_LCLIFF) / 2) + 100
c.RCLIFF_BW = (
(c.MAX_SENSOR_VALUE_RCLIFF + c.MIN_SENSOR_VALUE_RCLIFF) / 2) + 100
c.LFCLIFF_BW = (
(c.MAX_SENSOR_VALUE_LFCLIFF + c.MIN_SENSOR_VALUE_LFCLIFF) / 2) + 100
c.RFCLIFF_BW = (
(c.MAX_SENSOR_VALUE_RFCLIFF + c.MIN_SENSOR_VALUE_RFCLIFF) / 2) + 200
c.BASE_LM_POWER = int((total_left_speed * 2) / run_throughs)
c.BASE_RM_POWER = int((total_right_speed * 2) / run_throughs)
c.FULL_LM_POWER = c.BASE_LM_POWER
c.FULL_RM_POWER = c.BASE_RM_POWER
c.HALF_LM_POWER = int(c.BASE_LM_POWER) / 2
c.HALF_RM_POWER = int(c.BASE_RM_POWER) / 2
print "LCLIFF_BW: " + str(c.LCLIFF_BW)
print "RCLIFF_BW: " + str(c.RCLIFF_BW)
print "LFCLIFF_BW: " + str(c.LFCLIFF_BW)
print "RFCLIFF_BW: " + str(c.RFCLIFF_BW)
print "BASE_LM_POWER: " + str(c.BASE_LM_POWER)
print "BASE_RM_POWER: " + str(c.BASE_RM_POWER)
msleep(100)
s.backwards_until_black_cliffs()
s.align_far_cliffs()
s.turn_left_until_lfcliff_senses_black(0)
g.determine_gyro_conversion_rate()
msleep(100)
g.turn_right_gyro(45)
s.backwards_until_black_lfcliff()
s.align_far_fcliffs()
s.backwards_through_line_lfcliff()
s.align_close_fcliffs()
m.backwards(200)
msleep(300)
ao()
# DON'T DELETE THESE NEXT 4 LINES. They are purposeful. It avoids the roomba going into sleep mode after the calibration and not starting right.
create_disconnect()
msleep(4000)
#wait_for_light(c.LIGHT_SENSOR)
create_connect()
shut_down_in(120) # URGENT: PUT BACK IN BEFORE COMPETITION
