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"
