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 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 put_botguy_on_side(): print "Starting put_botguy_on_side()" approach_t() f.right_point_turn_until_third_senses_black(.2) 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.right_point_turn_until_left_senses_black(10, 0, 0, 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)
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 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 bump_right(): while get_create_rbump() == 0: m.drive(50)
def bump_left(): while get_create_lbump() == 0: m.drive(50)
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"
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(): # 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 / 2 * -1), int(c.BASE_RM_POWER / 2 * -1)) while gmpc(c.LEFT_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)) + 1000 else: # Clone bot c.THIRD_TOPHAT_BW = int( ((max_sensor_value_third + min_sensor_value_third) / 2)) + 1000 print "max_sensor_value_left: " + str(max_sensor_value_left) print "min_sensor_value_left: " + str(min_sensor_value_left) print "LEFT_TOPHAT_BW: " + 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 "RIGHT_TOPHAT_BW: " + 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 "THIRD_TOPHAT_BW: " + str(c.THIRD_TOPHAT_BW) c.MAX_TOPHAT_VALUE_RIGHT = max_sensor_value_right c.MIN_TOPHAT_VALUE_RIGHT = min_sensor_value_right c.MAX_TOPHAT_VALUE_LEFT = max_sensor_value_left c.MIN_TOPHAT_VALUE_LEFT = min_sensor_value_left print "Finished Calibrating. Moving back into starting box...\n" s.drive_until_black_left() s.align_close() s.drive_through_line_third(0) m.drive(1050) msleep(25) ao() msleep(2000) wait_for_light(c.LIGHT_SENSOR) shut_down_in(118) # URGENT: PUT BACK IN BEFORE COMPETITION
def sample_command(): m.drive(1000) left.lfollow_until(boolean=right.senses_black, mode=INSIDE_LINE)
def sample_command(): m.drive(500)