def test_servos(exit=True):
# Used to see if basic servo commands and constants function as intended.
print "Testing servos\n"
m.close_claw()
m.wait() # Using wait() instead of msleep() to make sure wheels are off.
m.open_claw()
m.wait()
m.lift_arm()
m.wait()
m.lower_arm()
m.wait()
print "Testing complete."
if exit == True:
print "Exiting...\n"
exit(86)
def get_gas_valve():
s.wfollow_right_until_black_left_front()
s.wfollow_right_until_white_left_front()
s.wfollow_right_until_black_left_front()
s.wfollow_right_until_black_right()
s.wfollow_right_until_white_right()
s.turn_left_until_rfcliff_senses_white()
s.turn_left_until_rfcliff_senses_black()
m.turn_left(c.LEFT_TURN_TIME / 4.8)
# This turns the robot closer to 90 degrees
s.forwards_until_white_lfcliff()
s.forwards_through_line_lfcliff()
s.align_far_fcliffs()
s.align_far_fcliffs()
s.forwards_until_bump()
m.backwards(1000)
#g.turn_left_gyro(180)
m.turn_left()
m.turn_left()
s.forwards_until_black_lfcliff()
s.align_close_fcliffs()
s.align_close_fcliffs()
#m.turn_left(250)
m.lower_arm()
msleep(500)
m.backwards(900)
m.lift_arm(3, 1, c.ARM_HALF_UP_POS)
m.backwards(300)
m.lift_arm()
s.forwards_until_black_lfcliff()
s.align_close_fcliffs()
s.forwards_until_bump()
m.backwards(100)
#g.turn_right_gyro()
m.turn_right(c.RIGHT_TURN_TIME / 1.2)
s.wfollow_left_until_white_right_front(9999)
s.wfollow_left_until_black_right_front(9999)
s.wfollow_left_until_white_right_front(9999)
s.align_far_fcliffs()
s.align_far_fcliffs()
m.backwards(500)
#g.turn_right_gyro(30)
m.turn_right(c.RIGHT_TURN_TIME / 3.5)
m.backwards(400)
m.lower_arm(1, 1, c.ARM_DELIVERY_POS)
shut_down_in(118)
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 hold_cube_and_deliver_ambulance():
print "Starting deliver_ambulance()"
s.backwards_until_black_left()
m.lift_arm()
s.drive_until_black_third()
s.turn_right_until_black()
s.backwards_through_line_third(0)
s.backwards_until_black_right()
m.lower_cube_arm()
s.drive_through_line_third(0)
s.drive_until_black_left()
m.close_claw()
m.move_cube_arm(c.CUBE_ARM_HOLDING_POS)
m.move_claw(c.CLAW_LESS_OPEN_POS)
m.move_cube_arm(c.CUBE_ARM_UP_POS)
m.turn_left(int(c.RIGHT_TURN_TIME / 1.5))
s.backwards_through_line_left(0)
s.backwards_through_line_third()
s.turn_right_until_left_senses_black(0)
s.turn_right_until_left_senses_white()
s.lfollow_left_until_right_senses_black_pid_safe_no_stop(500)
s.lfollow_left_until_right_senses_black_pid()
#s.lfollow_left_until_right_senses_black(1000)
#s.lfollow_left_until_right_senses_black_smooth()
s.turn_right_until_white(0)
s.turn_right_until_black()
m.lower_arm()
w.check_zones_hospital()
m.lift_arm()
if c.SAFE_HOSPITAL == c.NEAR_ZONE:
s.backwards_through_line_third()
m.lower_arm()
m.backwards(500)
s.drive_until_black_third()
m.lift_arm()
else: # Safe hospital is far zone
s.backwards_through_line_third()
s.turn_right_until_white(0)
s.turn_right_until_black()
s.backwards_until_white_right(0)
m.backwards(500)
m.lower_arm()
# Ambulance delivered
s.drive_through_line_third()
s.turn_right_until_left_senses_black(0)
s.turn_right_until_left_senses_white(0)
print "Finished delivering ambulance and yellow cube"