def setup(): # Enables servos and sets them to predefined starting positions. This goes before every run print "Starting setup()" if c.IS_MAIN_BOT: print "I am the main bot" elif c.IS_CLONE_BOT: print "I am the clone bot" else: print "Error in bot determination" exit(86) if c.STARTING_CLAW_POS > c.MAX_SERVO_POS or c.STARTING_CLAW_POS < c.MIN_SERVO_POS or c.STARTING_ARM_POS > c.MAX_SERVO_POS or c.STARTING_ARM_POS < c.MIN_SERVO_POS : print "Invalid desired servo position\n" exit(86) graphics_close() ao() enable_servo(c.CLAW_SERVO) enable_servo(c.ARM_SERVO) set_servo_position(c.CLAW_SERVO, c.STARTING_CLAW_POS) set_servo_position(c.ARM_SERVO, c.STARTING_ARM_POS) msleep(1000) print "Set claw to starting position of %d" % c.STARTING_CLAW_POS print "Set arm to starting position of %d" % c.STARTING_ARM_POS m.claw_slow(c.CLAW_PARALLEL_CLOSE_POS) m.wait() m.claw_slow(c.CLAW_OPEN_POS) m.wait() console_clear() print "Setup complete\n\n"
def test_servos(exit = True): # Used to see if basic servo commands and constants function as intended. print "Testing servos\n" m.claw_slow(c.CLAW_CLOSE_POS) m.wait() # Using wait() instead of msleep() to make sure wheels are off. m.claw_slow(c.CLAW_OPEN_POS) m.wait() m.arm_slow(c.ARM_DOWN_POS) m.wait() m.arm_slow(c.ARM_HIGH_POS) m.wait() print "Testing complete." if exit == True: print "Exiting...\n" exit(86)
def test_servos(exit=True): # Used to see if basic servo commands and constants function as intended. print "Testing servos\n" m.close_claw() m.wait() # Using wait() instead of msleep() to make sure wheels are off. m.open_claw() m.wait() m.lift_arm() m.wait() m.lower_arm() m.wait() print "Testing complete." if exit == True: print "Exiting...\n" exit(86)
def test_servos_extensive(exit = True): # Runs all constant servo positions for arm and claw. Not updated. print "Testing servos extensively\n" m.claw_slow(c.STARTING_CLAW_POS) m.arm_slow(c.STARTING_ARM_POS) m.wait() m.claw_slow(c.CLAW_OPEN_POS) m.wait() m.claw_slow(c.CLAW_CLOSE_POS) m.wait() m.claw_slow(c.CLAW_SECOND_CRATE_GRAB_POS) m.wait() m.claw_slow(c.BOTGUY_CLAW_CLOSE_POS) m.wait() m.arm_slow(c.ARM_UP_POS) m.wait() m.arm_slow(c.ARM_DOWN_POS) m.wait() m.arm_slow(c.ARM_SECOND_CRATE_UP_POS) m.wait() m.arm_slow(c.ARM_SECOND_CRATE_GRAB_POS) m.wait() m.arm_slow(c.ARM_DOWN_POS) m.wait() print "Testing complete." if exit == True: print "Exiting...\n" exit(86)
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"