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