def go_to_magnets():
g.forwards_gyro_until_bump()
g.backwards_gyro(15)
g.turn_left_gyro()
s.wfollow_right_until_black_left(0)
s.wfollow_right_until_white_left_front(0)
s.wfollow_right_until_white_left(0)
s.wfollow_right_until_black_left_front(0)
s.wfollow_right_until_black_left()
s.align_on_wall_right()
s.wfollow_right_smooth(800, should_stop=False)
s.wfollow_right_smooth_until_white_rfcliff(0)
s.wfollow_right_smooth_until_black_rfcliff(0)
s.wfollow_right_smooth_until_black_rcliff()
g.backwards_gyro_until_both_white_cliffs()
s.align_close_cliffs()
g.forwards_gyro_through_line_rcliff(0)
g.forwards_gyro(300, should_stop=False)
g.forwards_gyro_until_bump()
g.backwards_gyro(15)
g.turn_left_gyro()
s.wfollow_right_until_black_right(0)
s.wfollow_right(1200)
s.align_on_wall_right()
s.wfollow_right_smooth(800, should_stop=False)
s.wfollow_right_smooth_until_white_rcliff(0)
s.wfollow_right_smooth_until_black_rcliff(0)
s.wfollow_right_smooth_until_white_rcliff()
def deliver_ambulance_and_blocks():
w.check_zones_hospital()
if c.SAFE_HOSPITAL == c.NEAR_ZONE:
g.drive_gyro_through_line_right()
s.align_far()
m.open_claw(1, 1, c.CLAW_WAY_OPEN_POS)
msleep(500)
m.move_arm(c.ARM_HIGH_POS)
#m.move_claw(c.CLAW_WAY_OPEN_POS)
#w.close_graphics_window()
g.turn_left_gyro(190)
else:
u.halve_speeds()
g.drive_gyro_through_line_right()
s.align_far()
s.left_forwards_until_white(0)
s.left_forwards_until_black()
m.open_claw(1, 1, c.CLAW_WAY_OPEN_POS)
msleep(500)
m.move_arm(c.ARM_HIGH_POS)
#m.move_claw(c.CLAW_WAY_OPEN_POS)
u.normalize_speeds()
#w.close_graphics_window()
u.sd()
g.turn_left_gyro(190)
g.backwards_gyro_through_line_left(1100)
m.lower_ambulance_arm()
g.backwards_gyro(500)
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 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 do_magnets():
g.backwards_gyro_until_black_rcliff()
g.forwards_gyro(100)
m.lift_magnet_arm()
s.align_on_wall_right()
s.wfollow_right_smooth_until_black_rcliff()
g.backwards_gyro_until_white_rcliff(0)
msleep(1000)
g.backwards_gyro(50)
m.lower_magnet_arm()
g.backwards_gyro_until_black_lfcliff()
s.align_far_fcliffs()
g.backwards_gyro(80)
m.lift_magnet_arm()
g.backwards_gyro(1500)
msleep(1000)
g.forwards_gyro(50)
m.lower_magnet_arm()