def front_forwards_align_until_black(time): print "start front_forwards_align_until_black()" sec = seconds() + time while seconds() < sec: if NotBlackFrontLeft(): print "it works with left?" if get_create_lbump() == 1: m.backwards(80, -100, -100) m.pivot_left(20) else: create_drive_direct(-100, -100) elif NotBlackFrontRight(): print "it works with right?" if get_create_lbump() == 1: m.backwards(80, -100, -100) m.pivot_left(20) else: create_drive_direct(-100, -100) elif BlackFrontLeft(): print "sensed black with left cliff" exit(86) elif BlackFrontRight(): print "sensed black with right cliff" exit(86) msleep(100) create_stop()
def collect_poms(msleep_time=50, forwards_amount=1000, amount=1250): # Actual act of collecting poms once aligned m.backwards(amount) m.arm_slow(c.ARM_COLLECTION_POS) msleep(msleep_time) m.forwards(forwards_amount)
def get_crates(): print "Starting get_crates()" print "Bot in starting box\n" f.drive_through_two_lines_third() f.right_point_turn_until_black() f.lfollow_right_until_left_senses_black_smooth_amount( .5, 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_right_until_left_senses_black_smooth_amount( .5, c.BASE_LM_POWER, c.BASE_RM_POWER, int(.7 * c.BASE_LM_POWER), int(.7 * c.BASE_RM_POWER), c.BASE_LM_POWER, c.BASE_RM_POWER, False) f.lfollow_right_until_left_senses_black_smooth_amount( 10, c.BASE_LM_POWER, c.BASE_RM_POWER, int(.9 * c.BASE_LM_POWER), int(.9 * c.BASE_RM_POWER), c.BASE_LM_POWER, c.BASE_RM_POWER, False) print "Bot on center tee\n" 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(10, 0, 0, False) f.right_point_turn_until_third_senses_black(10, c.BASE_LM_POWER, -1 * c.BASE_RM_POWER) f.left_backwards_until_black() f.right_backwards_until_black() m.open_claw() m.arm_slow(c.ARM_DOWN_POS) m.claw_slow(c.CLAW_LESS_OPEN_POS) m.backwards(1400) print "Bot driving backwards to get crates\n" m.close_claw() print "\n\nFinished getting crates\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 get_low_poms_cheeky(): # First pom set print "Starting get_low_poms()" m.arm_slow(c.ARM_UP_POS, 3, 1) f.backwards_through_line_lfcliff() f.backwards_through_line_lfcliff() f.align_close_fcliffs() collect_poms(50, 0) f.forwards_until_black_lfcliff() m.backwards(150)
def begin_pom_conquest(time): sec = seconds() + time while seconds() < sec: if get_create_lbump() == 1: create_stop() m.backwards(80, -100, -100) m.pivot_left(20) else: create_drive_direct(-100, -100) create_stop() front_forwards_align_until_black(4)
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 calibrate(): # Code to calibrate the bw values. This goes before every run. Ends with light sensor calibration. max_sensor_value_right = 0 min_sensor_value_right = 90000 max_sensor_value_left = 0 min_sensor_value_left = 90000 max_sensor_value_third = 0 min_sensor_value_third = 90000 cmpc(c.LEFT_MOTOR) cmpc(c.RIGHT_MOTOR) if c.IS_MAIN_BOT: calibrate_tics = 2000 else: # Clone bot calibrate_tics = 2500 print "Running calibrate()" m.activate_motors(int (c.BASE_LM_POWER / 3), int(c.BASE_RM_POWER / 3)) while gmpc(c.LEFT_MOTOR) < calibrate_tics and gmpc(c.RIGHT_MOTOR) < calibrate_tics: if analog(c.RIGHT_TOPHAT) > max_sensor_value_right: max_sensor_value_right = analog(c.RIGHT_TOPHAT) if analog(c.RIGHT_TOPHAT) < min_sensor_value_right: min_sensor_value_right = analog(c.RIGHT_TOPHAT) if analog(c.LEFT_TOPHAT) > max_sensor_value_left: max_sensor_value_left = analog(c.LEFT_TOPHAT) if analog(c.LEFT_TOPHAT) < min_sensor_value_left: min_sensor_value_left = analog(c.LEFT_TOPHAT) if analog(c.THIRD_TOPHAT) > max_sensor_value_third: max_sensor_value_third = analog(c.THIRD_TOPHAT) if analog(c.THIRD_TOPHAT) < min_sensor_value_third: min_sensor_value_third = analog(c.THIRD_TOPHAT) msleep(1) m.deactivate_motors() c.LEFT_TOPHAT_BW = int(((max_sensor_value_left + min_sensor_value_left) / 2)) - 1000 c.RIGHT_TOPHAT_BW = int(((max_sensor_value_right + min_sensor_value_right) / 2)) - 1000 if c.IS_MAIN_BOT: c.THIRD_TOPHAT_BW = int(((max_sensor_value_third + min_sensor_value_third) / 2)) else: # Clone bot c.THIRD_TOPHAT_BW = int(((max_sensor_value_third + min_sensor_value_third) / 2)) + 300 print "max_sensor_value_left: " + str(max_sensor_value_left) print "min_sensor_value_left: " + str(min_sensor_value_left) print "Target Left " + str(c.LEFT_TOPHAT_BW) print "max_sensor_value_right: " + str(max_sensor_value_right) print "min_sensor_value_right: " + str(min_sensor_value_right) print "Target Right " + str(c.RIGHT_TOPHAT_BW) print "max_sensor_value_third: " + str(max_sensor_value_third) print "min_sensor_value_third: " + str(min_sensor_value_third) print "Target Third " + str(c.THIRD_TOPHAT_BW) f.backwards_through_two_lines_in_calibration() f.align_close() m.backwards(600) msleep(25) ao() msleep(1000)
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 main(): print "Starting main()\n" u.setup() u.calibrate() a.only_first_three() # m.backwards(1300) m.turn_right() m.backwards(4500) u.sd() a.get_low_poms_cheeky() a.get_frisbee() a.get_mid_poms() a.get_high_poms_cheeky() a.get_farther_high_poms() a.get_farther_mid_poms() a.get_farther_low_poms() print "Finished main\n" u.shutdown(86)
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 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 get_farther_high_poms(): print "Starting get_farther_high_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_until_rfcliff_senses_black() f.forwards_until_white_rfcliff() f.right_point_turn_until_rfcliff_senses_black(2) f.right_point_turn_until_lfcliff_senses_black(2) f.right_point_turn_until_rfcliff_senses_white(2) f.right_point_turn_until_rfcliff_senses_black(2) f.backwards_until_black_lfcliff() # m.turn_right(int(0.5 * c.RIGHT_TURN_TIME), 2 * c.BASE_LM_POWER, -2 * c.BASE_RM_POWER) # m.turn_right(int(0.5 * c.RIGHT_TURN_TIME), 2 * c.BASE_LM_POWER, -2 * c.BASE_RM_POWER) m.backwards(1650) m.turn_right() f.forwards_until_line_fcliffs() f.align_close_fcliffs() collect_poms()
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 calibrate(): max_sensor_value_rcliff = 0 min_sensor_value_rcliff = 90000 max_sensor_value_lcliff = 0 min_sensor_value_lcliff = 90000 max_sensor_value_lfcliff = 0 min_sensor_value_lfcliff = 90000 max_sensor_value_rfcliff = 0 min_sensor_value_rfcliff = 90000 sec = seconds() + 9 print "Running calibrate()" m.activate_motors(-1, int(c.BASE_LM_POWER / 2), int(c.BASE_RM_POWER / 2)) print str(int(c.BASE_LM_POWER / 2)) print str(int(c.BASE_RM_POWER / 2)) while seconds() < sec: if get_create_rcliff_amt() > max_sensor_value_rcliff: max_sensor_value_rcliff = get_create_rcliff_amt() if get_create_rcliff_amt() < min_sensor_value_rcliff: min_sensor_value_rcliff = get_create_rcliff_amt() if get_create_lcliff_amt() > max_sensor_value_lcliff: max_sensor_value_lcliff = get_create_lcliff_amt() if get_create_lcliff_amt() < min_sensor_value_lcliff: min_sensor_value_lcliff = get_create_lcliff_amt() if get_create_rfcliff_amt() > max_sensor_value_rfcliff: max_sensor_value_rfcliff = get_create_rfcliff_amt() if get_create_rfcliff_amt() < min_sensor_value_rfcliff: min_sensor_value_rfcliff = get_create_rfcliff_amt() if get_create_lfcliff_amt() > max_sensor_value_lfcliff: max_sensor_value_lfcliff = get_create_lfcliff_amt() if get_create_lfcliff_amt() < min_sensor_value_lfcliff: min_sensor_value_lfcliff = get_create_lfcliff_amt() msleep(1) m.deactivate_motors() c.LCLIFF_BW = ( (max_sensor_value_lcliff + min_sensor_value_lcliff) / 2) + 750 c.RCLIFF_BW = ( (max_sensor_value_rcliff + min_sensor_value_rcliff) / 2) + 750 c.LFCLIFF_BW = ( (max_sensor_value_lfcliff + min_sensor_value_lfcliff) / 2) + 770 c.RFCLIFF_BW = ( (max_sensor_value_rfcliff + min_sensor_value_rfcliff) / 2) + 1000 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 "max_sensor_value_rcliff: " + str(max_sensor_value_rcliff) print "min_sensor_value_rcliff: " + str(min_sensor_value_rcliff) msleep(500) f.forwards_until_black_lfcliff() f.align_close_fcliffs() msleep(300) f.forwards_until_black_lcliff() f.align_close_cliffs() msleep(300) f.forwards_until_bump() m.backwards(100) #m.activate_motors(1) #msleep(4250) #m.deactivate_motors() msleep(1500) ao() create_disconnect() wait_for_light(c.LIGHT_SENSOR) create_connect() shut_down_in(120) # URGENT: PUT BACK IN BEFORE COMPETITION
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 calibrate(): # 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 total_tophat_reading = 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() total_tophat_reading += analog(c.CLAW_TOPHAT) 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() # If sensing black when should be sensing white, decrease bias. # If sensing white when should be sensing black, increase bias 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) + 100 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) c.CLAW_TOPHAT_NOTHING_READING = total_tophat_reading / run_throughs #print "\nPut coupler in claw and press the right button..." #s.wait_until(isRightButtonPressed) #c.CLAW_TOPHAT_COUPLER_READING = analog(c.CLAW_TOPHAT) #c.CLAW_TOPHAT_BW = (c.CLAW_TOPHAT_COUPLER_READING + c.CLAW_TOPHAT_NOTHING_READING) / 2 #print "c.CLAW_TOPHAT_BW: " + str(c.CLAW_TOPHAT_BW) #if c.CLAW_TOPHAT_COUPLER_READING > c.CLAW_TOPHAT_NOTHING_READING: # print "Coupler reading is higher." #else: # print "Nothing reading is higher." s.backwards_until_black_cliffs() s.align_far_cliffs() 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() wait_for_light(c.LIGHT_SENSOR) create_connect() shut_down_in(120) # URGENT: PUT BACK IN BEFORE COMPETITION
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
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"
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 calibrate(): max_sensor_value_rcliff = 0 min_sensor_value_rcliff = 90000 max_sensor_value_lcliff = 0 min_sensor_value_lcliff = 90000 max_sensor_value_lfcliff = 0 min_sensor_value_lfcliff = 90000 max_sensor_value_rfcliff = 0 min_sensor_value_rfcliff = 90000 if c.BASE_LM_POWER == 0: print "c.BASE_LM_POWER can not equal 0 for the calibrate command. Autosetting to 109." c.BASE_LM_POWER = 109 sec = seconds() + (4 * 109 / c.BASE_LM_POWER) print "Running calibrate()" m.activate_motors(int(c.BASE_LM_POWER / 2), int(c.BASE_RM_POWER / 2)) print str(int(c.BASE_LM_POWER / 2)) print str(int(c.BASE_RM_POWER / 2)) while seconds() < sec: if get_create_rcliff_amt() > max_sensor_value_rcliff: max_sensor_value_rcliff = get_create_rcliff_amt() if get_create_rcliff_amt() < min_sensor_value_rcliff: min_sensor_value_rcliff = get_create_rcliff_amt() if get_create_lcliff_amt() > max_sensor_value_lcliff: max_sensor_value_lcliff = get_create_lcliff_amt() if get_create_lcliff_amt() < min_sensor_value_lcliff: min_sensor_value_lcliff = get_create_lcliff_amt() if get_create_rfcliff_amt() > max_sensor_value_rfcliff: max_sensor_value_rfcliff = get_create_rfcliff_amt() if get_create_rfcliff_amt() < min_sensor_value_rfcliff: min_sensor_value_rfcliff = get_create_rfcliff_amt() if get_create_lfcliff_amt() > max_sensor_value_lfcliff: max_sensor_value_lfcliff = get_create_lfcliff_amt() if get_create_lfcliff_amt() < min_sensor_value_lfcliff: min_sensor_value_lfcliff = get_create_lfcliff_amt() msleep(1) m.deactivate_motors() c.LCLIFF_BW = ( (max_sensor_value_lcliff + min_sensor_value_lcliff) / 2) + 500 c.RCLIFF_BW = ( (max_sensor_value_rcliff + min_sensor_value_rcliff) / 2) + 500 c.LFCLIFF_BW = ( (max_sensor_value_lfcliff + min_sensor_value_lfcliff) / 2) + 500 c.RFCLIFF_BW = ( (max_sensor_value_rfcliff + min_sensor_value_rfcliff) / 2) + 500 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 "max_sensor_value_rcliff: " + str(max_sensor_value_rcliff) print "min_sensor_value_rcliff: " + str(min_sensor_value_rcliff) msleep(500) s.backwards_until_black_cliffs() s.align_far_cliffs() s.turn_left_until_lfcliff_senses_black() msleep(300) g.calibrate_gyro_degrees() msleep(300) m.turn_right(int(c.RIGHT_TURN_TIME / 2)) s.backwards_until_black_lfcliff() s.align_far_fcliffs() msleep(300) m.backwards(600) 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() wait_for_light(c.LIGHT_SENSOR) create_connect() shut_down_in(120) # URGENT: PUT BACK IN BEFORE COMPETITION