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_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 hold_cube_and_get_prism():
print "Starting get_prism()"
if c.SAFE_HOSPITAL == c.NEAR_ZONE:
s.turn_right_until_left_senses_white(0)
s.turn_right_until_left_senses_black(0)
s.turn_right_until_black(0)
s.turn_right_until_left_senses_white(0)
s.turn_right_until_left_senses_black(0)
s.turn_right_until_left_senses_white()
s.lfollow_left_until_right_senses_black_pid()
m.lower_cube_arm()
s.drive_through_line_right(0)
s.align_far()
s.drive_until_black_right()
m.close_claw()
m.lift_cube_arm()
s.align_close()
m.close_claw()
m.lift_cube_arm()
s.turn_left_until_white(0)
s.turn_left_until_black()
s.lfollow_left_until_right_senses_black_pid(0)
s.drive_through_line_right(0)
s.drive_until_black_third()
m.turn_left_until__right_senses_black()
m.lower_cube_arm()
m.open_claw()
m.move_cube_arm(c.CUBE_ARM_LESS_UP)
m.move_claw(c.CLAW_LESS_OPEN_POS)
m.move_cube_arm(c.CUBE_ARM_HOLDING_POS)
m.close_claw()
m.lower_cube_arm()
m.open_claw()
else: # Safe hospital is far zone
s.lfollow_left_until_right_senses_black_smooth()
m.lower_cube_arm()
s.drive_through_line_right(0)
s.drive_until_black_right()
m.close_claw()
m.lift_cube_arm()
s.align_close()
s.turn_left_until_white(0)
s.turn_left_until_black(0)
s.turn_left_until_right_senses_black(0)
s.turn_left_until_right_senses_white()
m.lower_cube_arm()
m.open_claw()
m.move_cube_arm(c.CUBE_ARM_LESS_UP_POS)
m.move_claw(c.CLAW_LESS_OPEN_POS)
m.move_cube_arm(c.CUBE_ARM_HOLDING_POS)
m.close_claw()
m.move_cube_arm(c.CUBE_ARM_LESS_UP_POS)
m.open_claw()
print "Finished getting and delivering yellow prism"
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_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 get_blocks():
g.backwards_gyro_until_black_left()
s.align_far()
g.backwards_gyro_through_line_left(0)
g.backwards_gyro_until_black_left()
s.align_far()
g.backwards_gyro_through_line_left(0)
g.backwards_gyro(300)
g.turn_right_gyro(90)
g.drive_gyro(2000, should_stop=False)
g.drive_gyro_until_white_left(0)
g.drive_gyro_until_black_left(0)
g.drive_gyro(500, should_stop=False)
g.drive_gyro_until_white_left(0)
g.drive_gyro(950)
s.turn_left_until_black()
m.open_claw()
m.lower_arm()
#s.lfollow_left_pid(13000, bias=15, should_stop=False)
s.lfollow_left_pid_until_black_third(time=21000, bias=10)
m.close_claw()
g.backwards_gyro(800)
m.lift_arm(1, 1)
s.lfollow_left_pid_until_black_right(bias=10)