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)
enable_servo(c.CUBE_ARM_SERVO)
c.STARTING_ARM_POS = get_servo_position(c.ARM_SERVO)
print "STARTING_ARM_POS now: " + str(c.ARM_DOWN_POS)
m.move_claw(c.STARTING_CLAW_POS)
m.move_arm(c.STARTING_ARM_POS)
m.move_cube_arm(c.STARTING_CUBE_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.move_claw(c.CLAW_CHECKING_POS)
#m.wait()
#m.move_claw(c.STARTING_CLAW_POS)
#m.wait()
console_clear()
print "Setup complete\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 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)
START_TIME = seconds()
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()
msleep(100)
g.calibrate_gyro()
cmpc(c.LEFT_MOTOR)
cmpc(c.RIGHT_MOTOR)
cmpc(c.AMBULANCE_ARM_MOTOR)
enable_servo(c.CLAW_SERVO)
enable_servo(c.ARM_SERVO)
enable_servo(c.MICRO_SERVO)
#c.STARTING_ARM_POS = get_servo_position(c.ARM_SERVO)
#print "STARTING_ARM_POS now: " + str(c.ARM_DOWN_POS)
m.move_claw(c.CLAW_CHECKING_POS, 8, 1)
m.move_micro(c.MICRO_CHECKING_POS, 8, 1)
m.move_claw(c.STARTING_CLAW_POS, 8, 1)
m.move_arm(c.ARM_DOWN_POS, 8, 1)
m.move_micro(c.STARTING_MICRO_POS, 8, 1)
m.lower_ambulance_arm()
msleep(25)
ao()
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.move_claw(c.CLAW_CHECKING_POS)
#msleep(1000)
#m.move_claw(c.STARTING_CLAW_POS)
#msleep(1000)
console_clear()
print "Setup complete\n\n"
def test_servos_extensive(
exit=True
): # Runs all constant servo positions for arm and claw. Not updated.
print "Testing servos extensively\n"
m.move_claw(c.STARTING_CLAW_POS)
m.move_arm(c.STARTING_ARM_POS)
m.stop_for(1000)
m.move_claw(c.CLAW_OPEN_POS)
m.stop_for(1000)
m.move_claw(c.CLAW_CLOSE_POS)
m.stop_for(1000)
m.move_arm(c.ARM_UP_POS)
m.stop_for(1000)
m.move_arm(c.ARM_DOWN_POS)
m.stop_for(1000)
m.move_arm(c.ARM_DOWN_POS)
m.stop_for(1000)
print "Testing complete."
if exit == True:
print "Exiting...\n"
exit(86)