def get_blue_cube(): print "Starting get_blue_cube()" if c.SAFE_HOSPITAL == c.NEAR_ZONE: s.turn_right_until_black(0) s.turn_right_until_white(0) s.turn_right_until_black(0) s.turn_right_until_left_senses_black(0) s.turn_right_until_left_senses_white() s.lfollow_left_until_right_senses_black_pid(0) s.drive_through_line_right(0) s.drive_until_black_right() else: # Safe hospital is far zone m.lift_cube_arm() s.turn_right_until_black(0) s.turn_right_until_white(0) s.turn_right_until_black(0) s.turn_right_until_left_senses_black() s.align_close() m.lower_cube_arm() s.drive_until_white_third(0) s.drive_until_black_third(0) m.drive(900) # Drive until the blue cube is in claw m.close_claw() m.lift_cube_arm() m.turn_left() m.bumpfollow_right_until_left_senses_black(0) m.bumpfollow_right_until_left_senses_white() m.lower_cube_arm() m.open_claw() print "Finished delivering blue cube"
def hit_flare(cur_pos): fwd_count = 25 # around 5 sec bwd_count = 15 # around 3 sec ok_to_go = False while True: img_front = cam_front.read() img_down = cam_down.read() coords_front = gesture_detection.get_coord_from_detection(img_front) cur_pos[0], cur_pos[1], cur_pos[2] = localizer.get_pos(img_down, cur_pos[0], cur_pos[1], cur_pos[2]) x, y = coords_front[0][0], coords_front[0][1] if x < 700: movement.turn_right() continue elif x > 800: movement.turn_left() continue elif coords_front[3] >= 200: # change this after testing ok_to_go = True if ok_to_go is True and fwd_count is not 0: movement.move_fwd() fwd_count -= 1 elif bwd_count is not 0: movement.move_bwd() bwd_count -= 1 else: img_down = cam_down.read() return localizer.get_pos(img_down, cur_pos[0], cur_pos[1], cur_pos[2])
def check_zones_botguy(): print "Checking for red zone" msleep(25) ao() camera_open() initialize_camera() a.botguy_zone = 0 if get_object_area(c.RED, 0) > 50: # Testing to see if middle zone is red print "The middle zone is red." a.botguy_zone = c.MIDDLE else: m.turn_left(300) msleep(25) ao() msleep(1000) print "The middle zone is not red." initialize_camera() if get_object_area(c.RED, 0) > 100: # Testing to see if left zone is red a.botguy_zone = c.LEFT print "The left zone is red." else: print "Not on the left, so it must be on the right." a.botguy_zone = c.RIGHT # If the middle and left zones are not red, then the right zone must be red. m.turn_right(300) msleep(25) ao() print a.botguy_zone camera_close() print "~fin~\n\n\n"
def check_zones_crates(): print "Checking zones for yellow and red" msleep(25) ao() camera_open() initialize_camera() a.crate_zone = 0 if get_object_area(c.YELLOW, 0) > 50: # Testing to see if middle zone is yellow print "The middle zone is yellow." a.crate_zone = c.MIDDLE else: m.turn_left(300) msleep(25) ao() msleep(1000) print "The middle zone is not yellow." initialize_camera() if get_object_area(c.YELLOW, 0) > 100: # Testing to see if left zone is yellow print "The left zone is yellow." a.crate_zone = c.LEFT else: print "Not on the left, so it must be on the right." a.crate_zone = c.RIGHT # If the middle and left zones are not yellow, then the right zone must be yellow. m.turn_right(300) msleep(25) ao() camera_close() print a.crate_zone print "~fin~\n\n\n"
def get_botguy(): print "Starting get_botguy()" if crate_zone == c.LEFT: print "I'm in the left zone and going to botguy" f.drive_through_line_left() # Bot on middle line f.snap_to_line_left() f.lfollow_left_until_right_senses_black_smooth(20, 0, 0, False) f.drive_until_white(5, c.BASE_LM_POWER, c.BASE_RM_POWER, False) f.lfollow_left_until_third_senses_black_smooth(5, c.BASE_LM_POWER, c.BASE_RM_POWER, False) m.drive_tics(300, c.BASE_LM_POWER, c.BASE_RM_POWER) f.left_point_turn_until_third_senses_white(5 * c.LEFT_TURN_TIME, 0, 0) elif crate_zone == c.MIDDLE: print "I'm in the middle zone and going to botguy" m.turn_right() m.drive(1500) m.turn_left() f.drive_through_line_left() # Bot on middle line f.align_far() f.snap_to_line_left() f.lfollow_left_until_right_senses_black_smooth(20, 0, 0, False) f.drive_until_white(5, c.BASE_LM_POWER, c.BASE_RM_POWER, False) f.lfollow_left_until_third_senses_black_smooth(5, c.BASE_LM_POWER, c.BASE_RM_POWER, False) m.drive_tics(300, c.BASE_LM_POWER, c.BASE_RM_POWER) f.left_point_turn_until_third_senses_white(5 * c.LEFT_TURN_TIME, 0, 0) elif crate_zone == c.RIGHT: print "I'm in the right zone and going to botguy" f.drive_through_line_left() # Bot on middle line f.align_far() f.snap_to_line_right() f.lfollow_right_until_left_senses_black_smooth(20, 0, 0, False) if c.IS_MAIN_BOT: m.drive_tics(1007, c.BASE_LM_POWER, c.BASE_RM_POWER) else: # Clone bot m.drive_tics(1120, c.BASE_LM_POWER, c.BASE_RM_POWER) f.right_point_turn_until_third_senses_white( 5 * c.RIGHT_TURN_TIME / 1000, 0, 0, False) f.right_point_turn_until_third_senses_black(10, c.BASE_LM_POWER, -1 * c.BASE_RM_POWER) else: print "What zone am I in again? I have no idea. This is an error message" print "You already know I'm guessing it's in that right zone tho\n" exit(86) f.left_backwards_until_black() f.right_backwards_until_black() m.open_claw(c.CLAW_BOTGUY_OPEN_POS) m.arm_slow(c.ARM_DOWN_POS) #f.right_point_turn_until_third_senses_black() # To do: Run more tests on this command f.lfollow_backwards_smooth(4.5) m.arm_slow(c.ARM_UP_POS) m.backwards(490) m.arm_slow(c.ARM_PUSH_CRATE_POS, 2, 1) m.close_claw(c.CLAW_CLOSE_POS) m.arm_slow(c.ARM_DOWN_POS) m.backwards(100) m.close_claw(c.BOTGUY_CLAW_CLOSE_POS) print "Finished getting botguy\n\n"
def forwards_until_white_lfcliff_safe(): print "Start drive_until_white_lfcliff_safe" m.base_forwards() while BlackFrontLeft(): if BumpedRight(): m.turn_left(100) msleep(100) m.base_forwards() m.deactivate_motors()
def magnet(): m.turn_left() s.forwards_until_black_rfcliff_safe() s.forwards_until_white_rfcliff_safe() s.forwards_until_black_rfcliff_safe() s.align_close_fcliffs() m.turn_right() s.align_close_fcliffs() m.turn_right() s.lfollow_lfcliff_smooth_until_bump()
def get_mid_poms(): # Second pom set print "Starting get_mid_poms()" f.forwards_until_black_lfcliff() f.align_close_fcliffs() m.arm_slow(c.ARM_UP_POS, 2, 1) m.turn_left() m.backwards(2680) m.turn_right() f.forwards_until_black_lfcliff() f.align_close_fcliffs() collect_poms()
def get_farther_low_poms(): f.forwards_until_line_fcliffs() f.align_close_cliffs() m.arm_slow(c.ARM_UP_POS, 2, 1) m.turn_right(int(c.RIGHT_TURN_TIME * 1.01)) f.forwards_until_bump() m.backwards(200) m.turn_left() f.backwards_through_line_lfcliff() f.align_close_cliffs() collect_poms(50, 1000, 2100)
def test_movement(): # Used to see if movements and their defaults function as intended print "Testing movement\n" m.forward() msleep(500) # Using msleep() instead of wait() to make sure each command turns off its wheels m.backwards() msleep(500) m.turn_left() msleep(500) m.turn_right() msleep(500) print "Testing complete. Exiting...\n" exit(86)
def get_high_poms_cheeky(): print "Starting get_high_poms_cheeky()" f.forwards_until_black_lfcliff() f.align_close_fcliffs() m.arm_slow(c.ARM_UP_POS, 2, 1) m.turn_right() m.forwards(2680) m.turn_left() f.forwards_until_black_lfcliff() f.align_close_fcliffs() collect_poms(10000) f.backwards_until_white_lfcliff() m.arm_slow(c.ARM_UP_POS, 2, 1)
def get_farther_mid_poms(): f.forwards_until_line_fcliffs() f.align_close_cliffs() m.arm_slow(c.ARM_UP_POS, 2, 1) f.right_point_turn_until_lfcliff_senses_black() f.right_point_turn_until_lfcliff_senses_white() f.lfollow_lfcliff_smooth(2) f.lfollow_lfcliff_smooth_until_right_depth_senses_object() m.forwards(1000) m.turn_left() f.backwards_through_line_lfcliff() f.align_close_fcliffs() collect_poms()
def get_frisbee(): print "Starting get_frisbee()" m.arm_slow(c.ARM_UP_POS, 2, 1) m.backwards(900) m.arm_slow(c.ARM_FRISBEE_GRAB_POS, 2, 1) m.forwards(2000) m.arm_slow(c.ARM_FRISBEE_DROP_POS, 4, 1) m.turn_left( int(c.LEFT_TURN_TIME / 5)) # An 18 degree wiggle back and forth to shake frisbee loose m.turn_right(int(c.RIGHT_TURN_TIME / 5)) m.backwards(1000) m.lift_arm(c.ARM_FRISBEE_FARTHER_DROP_POS)
def get_high_poms(): # Third pom set print "Starting get_high_poms()" f.forwards_until_line_fcliffs() f.align_close_cliffs() f.right_point_turn_until_lfcliff_senses_black() f.right_point_turn_until_lfcliff_senses_white() # m.turn_right() # f.left_point_turn_until_lfcliff_senses_black() # How does it know it is on the right side of the line? f.lfollow_lfcliff_smooth_until_rfcliff_senses_black() m.backwards(1400) m.turn_left() f.backwards_through_line_lfcliff() f.align_close_fcliffs() collect_poms(50, 0)
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"
def test_movement(exit = True): # Used to see if movements and their defaults function as intended. print "Testing movement\n" m.turn_left() msleep(500) m.turn_right() msleep(500) m.drive(5000) msleep(500) # Using msleep() instead of wait() to make sure each command turns off its wheels. m.backwards(5000) msleep(500) print "Testing complete." if exit == True: print "Exiting...\n" exit(86)
def wfollow_right_choppy(time): sec = seconds() + time / 1000 while seconds() < sec: if isRoombaBumped(): if isLeftBumped(): m.backwards(100) m.turn_left() else: if c.FIRST_BUMP == True: m.deactivate_motors() u.halve_speeds() m.base_turn_left() c.FIRST_BUMP = False else: m.base_veer_right(0.6) c.FIRST_BUMP = True u.normalize_speeds() msleep(c.LFOLLOW_REFRESH_RATE) u.normalize_speeds() if should_stop: m.deactivate_motors()
def test_movement_extensive(exit = True): print "Extensively testing movement" mav(c.RIGHT_MOTOR, c.BASE_RM_POWER) msleep(c.PIVOT_RIGHT_TURN_TIME) mav(c.RIGHT_MOTOR,0) msleep(500) mav(c.LEFT_MOTOR, c.BASE_LM_POWER) msleep(c.PIVOT_LEFT_TURN_TIME) mav(c.LEFT_MOTOR,0) msleep(500) m.turn_left() msleep(500) m.turn_right() msleep(500) m.drive(1000) msleep(500) # Using msleep() instead of wait() to make sure each command turns off its wheels. m.backwards(1000) msleep(500) if exit == True: print "Exiting...\n" exit(86)
def wfollow_right_choppy_until(boolean_function, *, time=c.SAFETY_TIME): sec = seconds() + time / 1000 while seconds() < sec and not(boolean_function()): if isRoombaBumped(): if isLeftBumped(): m.backwards(100) m.turn_left() else: if c.FIRST_BUMP == True: m.deactivate_motors() u.halve_speeds() m.base_turn_left() c.FIRST_BUMP = False else: m.base_veer_right(0.6) c.FIRST_BUMP = True u.normalize_speeds() msleep(c.LFOLLOW_REFRESH_RATE) u.normalize_speeds() if should_stop: m.deactivate_motors()
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 check_zones_full(): print "Checking zones" msleep(25) ao() m.arm_slow(c.ARM_HIGH_POS, 2, 1) camera_open() initialize_camera() console_clear() graphics_open(480, 260) graphics_fill(255, 255, 255) graphics_update() a.crate_zone = 30 a.botguy_zone = 30 if get_object_area(c.YELLOW, 0) > 50: # Testing to see if middle zone is yellow print "The middle zone is yellow.\n" a.crate_zone = c.MIDDLE graphics_rectangle_fill(160, 0, 320, 260, 255, 255, 0) # Yellow graphics_update() elif get_object_area(c.RED, 0) > 50: a.botguy_zone = c.MIDDLE graphics_rectangle_fill(160, 0, 320, 260, 255, 0, 0) # Red graphics_update() print "The middle zone is red.\n" else: print "The middle is not red nor yellow.\n" graphics_rectangle_fill(160, 0, 320, 260, 0, 255, 0) # Green graphics_update() m.turn_left(400) # Checking the left zone msleep(25) ao() initialize_camera() if get_object_area(c.YELLOW, 0) > 100: # Testing to see if left zone is yellow print "The left zone is yellow.\n" graphics_rectangle_fill(0, 0, 160, 260, 255, 255, 0) # Yellow graphics_update() if a.crate_zone == 30: a.crate_zone = c.LEFT else: print "ERROR. The wallaby sensed yellow for the left zone and the middle zone.\n" elif get_object_area(c.RED, 0) > 100: print "The left zone is red\n" graphics_rectangle_fill(0, 0, 160, 260, 255, 0, 0) # Red graphics_update() if a.botguy_zone == 30: a.botguy_zone = c.LEFT else: print "ERROR. The wallaby sensed red for the left zone and the middle zone.\n" else: print "The left zone is neither red nor yellow.\n" graphics_rectangle_fill(0, 0, 160, 260, 0, 255, 0) # Green graphics_update() if a.crate_zone == 30: # If the crate zone has not been set yet. a.crate_zone = c.RIGHT graphics_rectangle_fill(320, 0, 480, 260, 255, 255, 0) # Yellow graphics_update() print "The right zone is yellow.\n" elif a.botguy_zone == 30: if a.crate_zone == c.RIGHT: print "Code failure" else: a.botguy_zone = c.RIGHT graphics_rectangle_fill(320, 0, 480, 260, 255, 0, 0) # Red graphics_update() print "The right zone is red. \n" else: print "The right zone is neither red nor yellow." graphics_rectangle_fill(320, 0, 480, 260, 0, 255, 0) # Green graphics_update() if a.botguy_zone == 30: if a.crate_zone == c.RIGHT: a.botguy_zone = c.LEFT graphics_rectangle_fill(0, 0, 160, 260, 255, 0, 0) # Red graphics_update() print "We are guessing that the left zone is red. \n" else: a.botguy_zone = c.RIGHT graphics_rectangle_fill(320, 0, 480, 260, 255, 0, 0) graphics_update() print "We are guessing that the right zone is red. \n" f.left_forwards_until_black( ) # The left tophat can end up on white - Jet 6-27-18 f.align_far() camera_close() graphics_close() print "Crate zone: " + str(a.crate_zone) print "Botguy zone: " + str(a.botguy_zone) print "~fin~\n\n\n"
def test_turns(): m.turn_left() msleep(500) m.turn_right() sd()
def deliver_second_crate(): print "Starting second_crate_delivery()" m.drive(250) m.arm_slow(c.ARM_SECOND_CRATE_GRAB_POS, 2, 1) m.backwards(100) m.claw_slow(c.CLAW_SECOND_CRATE_GRAB_POS, 3, 1) m.lift_arm(c.ARM_SECOND_CRATE_UP_POS) m.wait(100) if crate_zone == c.LEFT: f.left_point_turn_until_black() if c.IS_MAIN_BOT: f.lfollow_left_smooth_amount(.7, c.BASE_LM_POWER, c.BASE_RM_POWER, int(.5 * c.BASE_LM_POWER), int(.5 * c.BASE_RM_POWER), 0, 0, False) f.lfollow_left_smooth(.8, c.BASE_LM_POWER, c.BASE_RM_POWER) else: # Clone bot f.lfollow_left_smooth(1.5) f.left_point_turn_until_right_senses_black() f.lfollow_right_inside_line_until_left_senses_black_smooth_amount( 1, c.BASE_LM_POWER, c.BASE_RM_POWER, int(.4 * c.BASE_LM_POWER), int(.4 * c.BASE_RM_POWER), 0, 0, False) f.lfollow_right_inside_line_until_left_senses_black_smooth( 10, 0, 0, False) m.drive(150, c.BASE_LM_POWER, c.BASE_RM_POWER, c.BASE_LM_POWER, c.BASE_RM_POWER) m.turn_right() f.align_in_zone_safely() elif crate_zone == c.MIDDLE: f.right_point_turn_until_black() f.lfollow_right_smooth(1.5) f.right_point_turn_until_left_senses_black() f.lfollow_left_inside_line_until_right_senses_black_smooth_amount( 1, c.BASE_LM_POWER, c.BASE_RM_POWER, int(.4 * c.BASE_LM_POWER), int(.4 * c.BASE_RM_POWER), 0, 0, False) f.lfollow_left_inside_line_until_right_senses_black_smooth( 10, 0, 0, False) m.drive(180, c.BASE_LM_POWER, c.BASE_RM_POWER, c.BASE_LM_POWER, c.BASE_RM_POWER) f.right_point_turn_until_black(5, 0, 0, False) f.right_point_turn_until_white(5, c.BASE_LM_POWER, -1 * c.BASE_RM_POWER, False) f.right_point_turn_until_black(5, c.BASE_LM_POWER, -1 * c.BASE_RM_POWER, False) f.right_point_turn_until_left_senses_black(5, c.BASE_LM_POWER, -1 * c.BASE_RM_POWER, False) m.turn_right(c.RIGHT_TURN_TIME, c.BASE_LM_POWER, -1 * c.BASE_RM_POWER, c.BASE_LM_POWER, -1 * c.BASE_RM_POWER) f.align_in_zone_safely() elif crate_zone == c.RIGHT: f.drive_until_white_third(10, 0, 0, False) m.drive(300, c.BASE_LM_POWER, c.BASE_RM_POWER, c.BASE_LM_POWER, c.BASE_RM_POWER) f.right_point_turn_until_black() f.lfollow_right(3) f.right_point_turn_until_left_senses_black() f.lfollow_left_inside_line_until_right_senses_black_smooth_amount( 2, c.BASE_LM_POWER, c.BASE_RM_POWER, int(.4 * c.BASE_LM_POWER), int(.4 * c.BASE_RM_POWER), 0, 0, False) f.lfollow_left_inside_line_until_right_senses_black_smooth( 10, c.BASE_LM_POWER, c.BASE_RM_POWER, False) m.drive(200, c.BASE_LM_POWER, c.BASE_RM_POWER, c.BASE_LM_POWER, c.BASE_RM_POWER) m.turn_left() f.align_in_zone_safely() f.drive_through_line_third(10, 0, 0, False) m.drive(800, c.BASE_LM_POWER, c.BASE_RM_POWER, c.BASE_LM_POWER, c.BASE_RM_POWER) m.arm_slow(c.ARM_SECOND_CRATE_DEPOSIT_POS, 2, 1) m.claw_slow(c.CLAW_LARGE_OPEN_POS, 3, 1) m.arm_slow(c.ARM_PUSH_CRATE_POS, 2, 1) m.backwards(600) msleep(10) m.drive(200) m.arm_slow(c.ARM_HIGH_POS, 2, 1) print "Second crate delivered\n\n"
def deliver_first_crate(): print "Starting first_crate_delivery()" m.drive(500) if crate_zone == c.LEFT: print "Starting deliver_left()" f.snap_to_line_left() f.lfollow_left_until_right_senses_black_smooth(13, c.BASE_LM_POWER, c.BASE_RM_POWER, False) f.lfollow_left_smooth(1, c.BASE_LM_POWER, c.BASE_RM_POWER) f.right_point_turn_until_left_senses_white(10, 0, 0, False) f.right_point_turn_until_left_senses_black(10, c.BASE_LM_POWER, -1 * c.BASE_RM_POWER, False) f.right_point_turn_until_left_senses_white(10, c.BASE_LM_POWER, -1 * c.BASE_RM_POWER, False) f.right_point_turn_until_left_senses_black(10, c.BASE_LM_POWER, -1 * c.BASE_RM_POWER, False) f.right_point_turn_until_left_senses_white(10, c.BASE_LM_POWER, -1 * c.BASE_RM_POWER) f.lfollow_left_until_right_senses_black_smooth(7) if c.IS_MAIN_BOT: f.right_point_turn_until_black_after(c.RIGHT_TURN_TIME, 0, 0, False) f.right_point_turn_until_white(1, c.BASE_LM_POWER, -1 * c.BASE_RM_POWER) else: # Clone bot f.right_point_turn_until_black_after(c.RIGHT_TURN_TIME) elif crate_zone == c.MIDDLE: print "Starting deliver_middle()" f.snap_to_line_right() f.lfollow_right_until_left_senses_black_smooth(13, c.BASE_LM_POWER, c.BASE_RM_POWER, False) f.lfollow_right_smooth(1, c.BASE_LM_POWER, c.BASE_RM_POWER) f.left_point_turn_until_right_senses_white(10, 0, 0, False) f.left_point_turn_until_right_senses_black(10, -1 * c.BASE_LM_POWER, c.BASE_RM_POWER, False) f.left_point_turn_until_right_senses_white(10, -1 * c.BASE_LM_POWER, c.BASE_RM_POWER, False) f.left_point_turn_until_right_senses_black(10, -1 * c.BASE_LM_POWER, c.BASE_RM_POWER, False) f.left_point_turn_until_right_senses_white(10, -1 * c.BASE_LM_POWER, c.BASE_RM_POWER) f.lfollow_right_until_left_senses_black_smooth(7) f.drive_until_white_left() f.right_forwards_until_left_senses_black(10, 0, False) f.right_forwards_until_left_senses_white(10, c.BASE_RM_POWER) f.lfollow_left_inside_line_until_right_senses_black_smooth( 10, 0, 0, False) #f.left_forwards_until_black() f.drive_until_white_right(2, c.BASE_LM_POWER, c.BASE_RM_POWER, False) if c.IS_MAIN_BOT: f.lfollow_left_inside_line_smooth(2.5, c.BASE_LM_POWER, c.BASE_RM_POWER) else: # Clone bot f.lfollow_left_inside_line_smooth(2.7, c.BASE_LM_POWER, c.BASE_RM_POWER) m.turn_left() f.align_in_zone_safely() f.drive_through_line_third(10, 0, 0, False) m.drive(700, c.BASE_LM_POWER, c.BASE_RM_POWER, c.BASE_LM_POWER, c.BASE_RM_POWER) elif crate_zone == c.RIGHT: print "Starting deliver_right()" f.snap_to_line_right() f.lfollow_right_until_left_senses_black_smooth(13, c.BASE_LM_POWER, c.BASE_RM_POWER, False) f.lfollow_right_smooth(1, c.BASE_LM_POWER, c.BASE_RM_POWER) f.left_point_turn_until_right_senses_white(10, 0, 0, False) f.left_point_turn_until_right_senses_black(10, -1 * c.BASE_LM_POWER, c.BASE_RM_POWER, False) f.left_point_turn_until_right_senses_white(10, -1 * c.BASE_LM_POWER, c.BASE_RM_POWER, False) f.left_point_turn_until_right_senses_black(10, -1 * c.BASE_LM_POWER, c.BASE_RM_POWER, False) f.left_point_turn_until_right_senses_white(10, -1 * c.BASE_LM_POWER, c.BASE_RM_POWER) f.lfollow_right_until_left_senses_black_smooth(7) f.left_point_turn_until_black_after(c.LEFT_TURN_TIME, 0, 0, False) f.left_point_turn_until_white(1, -1 * c.BASE_LM_POWER, c.BASE_RM_POWER) m.backwards(200) m.arm_slow(c.ARM_DOWN_POS) msleep(300) # This pause helps keep the second crate from tipping over m.claw_slow(c.CLAW_LARGE_OPEN_POS) m.arm_slow(c.ARM_PUSH_CRATE_POS, 2, 1) m.backwards(500) print "First crate delivered\n\n"
def put_botguy_in_correct_zone(): approach_t() f.right_point_turn_until_third_senses_black(.2) print "Starting deliver_botguy()" if botguy_zone == c.LEFT: print "Starting deliver_left()" f.snap_to_line_left() f.lfollow_left_until_right_senses_black_smooth(13, c.BASE_LM_POWER, c.BASE_RM_POWER, False) f.lfollow_left_smooth(1, c.BASE_LM_POWER, c.BASE_RM_POWER) f.right_point_turn_until_left_senses_white(10, 0, 0, False) f.right_point_turn_until_left_senses_black(10, c.BASE_LM_POWER, -1 * c.BASE_RM_POWER, False) f.right_point_turn_until_left_senses_white(10, c.BASE_LM_POWER, -1 * c.BASE_RM_POWER, False) f.right_point_turn_until_left_senses_black(10, c.BASE_LM_POWER, -1 * c.BASE_RM_POWER, False) f.right_point_turn_until_left_senses_white(10, c.BASE_LM_POWER, -1 * c.BASE_RM_POWER) f.lfollow_left_until_right_senses_black_smooth(7) f.right_point_turn_until_black_after(c.RIGHT_TURN_TIME, 0, 0, False) m.turn_right(int(c.RIGHT_TURN_TIME / 2), c.BASE_LM_POWER, -1 * c.BASE_RM_POWER, c.BASE_LM_POWER, -1 * c.BASE_RM_POWER) m.claw_slow(c.CLAW_LARGE_OPEN_POS, 3, 1) elif botguy_zone == c.MIDDLE: print "Starting deliver_middle()" f.drive_until_white_third(5, 0, 0, False) m.drive(300, c.BASE_LM_POWER, c.BASE_RM_POWER, c.BASE_LM_POWER, c.BASE_RM_POWER) f.left_point_turn_until_right_senses_black(10, 0, 0, False) f.left_point_turn_until_right_senses_white(10, -1 * c.BASE_LM_POWER, c.BASE_RM_POWER, False) f.left_point_turn_until_right_senses_black(10, -1 * c.BASE_LM_POWER, c.BASE_RM_POWER) f.backwards_through_line_left() f.align_close() if False: # crate_zone == c.LEFT: m.turn_right(int(c.RIGHT_TURN_TIME / 8)) m.backwards(900) m.turn_right(int(c.RIGHT_TURN_TIME / 10)) elif False: # crate_zone == c.RIGHT: m.turn_left(int(c.LEFT_TURN_TIME / 8)) m.backwards(900) m.turn_left(int(c.LEFT_TURN_TIME / 20)) m.claw_slow(c.CLAW_LARGE_OPEN_POS, 3, 1) m.backwards(1000) elif botguy_zone == c.RIGHT: print "Starting deliver_right()" f.snap_to_line_right() f.lfollow_right_until_left_senses_black_smooth(13, c.BASE_LM_POWER, c.BASE_RM_POWER, False) f.lfollow_right_smooth(1, c.BASE_LM_POWER, c.BASE_RM_POWER) f.left_point_turn_until_right_senses_white(10, 0, 0, False) f.left_point_turn_until_right_senses_black(10, -1 * c.BASE_LM_POWER, c.BASE_RM_POWER, False) f.left_point_turn_until_right_senses_white(10, -1 * c.BASE_LM_POWER, c.BASE_RM_POWER, False) f.left_point_turn_until_right_senses_black(10, -1 * c.BASE_LM_POWER, c.BASE_RM_POWER, False) f.left_point_turn_until_right_senses_white(10, -1 * c.BASE_LM_POWER, c.BASE_RM_POWER) f.lfollow_right_until_left_senses_black_smooth(7) m.turn_left(c.LEFT_TURN_TIME, -1 * c.BASE_LM_POWER, c.BASE_RM_POWER, -1 * c.BASE_LM_POWER, c.BASE_RM_POWER, False) m.turn_left(c.LEFT_TURN_TIME, -1 * c.BASE_LM_POWER, c.BASE_RM_POWER, -1 * c.BASE_LM_POWER, c.BASE_RM_POWER, False) m.turn_left(int(c.LEFT_TURN_TIME / 2), -1 * c.BASE_LM_POWER, c.BASE_RM_POWER, -1 * c.BASE_LM_POWER, c.BASE_RM_POWER) m.claw_slow(c.CLAW_LARGE_OPEN_POS, 3, 1) m.arm_slow(c.ARM_PUSH_CRATE_POS, 3, 1) m.backwards(300) print "Botguy delivered\n\n"