Ejemplo n.º 1
0
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"
Ejemplo n.º 4
0
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"
Ejemplo n.º 5
0
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)