Beispiel #1
0
def move_to_vector(ser, vector, verbose = VERBOSE, num_error_attempts = NUM_ERROR_ATTEMPTS):
    """
    Move multiple servos concurrently. 
    
    :param ser: The ``serial`` object to use. 
    :param vector: A list of vectors in the form (id, position, velocity).  
    :param verbose: If True, status information will be printed. Default: ``VERBOSE``.
    :param num_error_attempts: The number of attempts to make to send the packet when an error is encountered.
    
    :returns: ``None``.
    
    """
    
    for i in range(0,len(vector)):
        
        servo_id = vector[i][0]
        angle = vector[i][1]
        velocity = vector[i][2]
        
        if verbose:
            print('Setting angle for {0} to {1}...'.format(id, angle))
        packet = packets.get_write_position_packet(servo_id, angle)
        dynamixel.write_and_get_response_multiple(ser, packet, id, verbose, num_error_attempts)
        
        if verbose:
            print('Setting velocity for {0} to {1}...'.format(id, velocity))
        packet = packets.get_write_velocity_packet(servo_id, velocity)
        dynamixel.write_and_get_response_multiple(ser, packet, id, verbose, num_error_attempts)
        
    if verbose:
        print('Sending action packet.')
    
    packet = packets.get_action_packet()
    ser.write(packet)
Beispiel #2
0
def wait_for_move(ser,
                  joints,
                  verbose=VERBOSE,
                  num_error_attempts=NUM_ERROR_ATTEMPTS):
    """
    Blocks until all of the servos with IDs specified by `joints` have stopped moving.

    :param ser: The ``serial`` port to use.
    :param joints: A list of servo IDs.
    :param verbose: If True, status information will be printed. Default: ``VERBOSE``.
    :param num_error_attempts: The number of attempts to make to send the packet when an error is encountered.

    :returns: ``None``.

    """

    # Iterate over the joints
    for j in joints:
        # Get the packet which will be used to read the moving status of the joint
        packet = packets.get_read_packet(j, 0x2E, 2)

        # Loop until the move status indicates that the joint is no longer moving
        while True:
            resp = dynamixel.write_and_get_response_multiple(
                ser, packet, j, verbose, num_error_attempts)

            moving = resp.data[0]

            if moving == 0:
                break

            time.sleep(SLEEP_TIME)
Beispiel #3
0
def wait_for_move(ser, joints, verbose = VERBOSE, num_error_attempts = NUM_ERROR_ATTEMPTS):
    """
    Blocks until all of the servos with IDs specified by `joints` have stopped moving.
    
    :param ser: The ``serial`` port to use. 
    :param joints: A list of servo IDs. 
    :param verbose: If True, status information will be printed. Default: ``VERBOSE``.
    :param num_error_attempts: The number of attempts to make to send the packet when an error is encountered.
    
    :returns: ``None``.
    
    """
       
    # Iterate over the joints 
    for j in joints:
        # Get the packet which will be used to read the moving status of the joint
        packet = packets.get_read_packet(j, 0x2E, 2)
                
        # Loop until the move status indicates that the joint is no longer moving
        while True:
            resp = dynamixel.write_and_get_response_multiple(ser, packet, j, verbose, num_error_attempts)
            
            moving = resp.data[0]
            
            if moving == 0:
                break
                        
            time.sleep(SLEEP_TIME)
Beispiel #4
0
def move_to_vector(ser,
                   vector,
                   verbose=VERBOSE,
                   num_error_attempts=NUM_ERROR_ATTEMPTS):
    """
    Move multiple servos concurrently.

    :param ser: The ``serial`` object to use.
    :param vector: A list of vectors in the form (id, position, velocity).
    :param verbose: If True, status information will be printed. Default: ``VERBOSE``.
    :param num_error_attempts: The number of attempts to make to send the packet when an error is encountered.

    :returns: ``None``.

    """

    for i in range(0, len(vector)):

        servo_id = vector[i][0]
        angle = vector[i][1]
        velocity = vector[i][2]

        if verbose:
            print('Setting angle for {0} to {1}...'.format(id, angle))
        print("BBBBBBBBBB\n", vector[i])
        print("AAAAAAA", angle)
        packet = packets.get_write_position_packet(servo_id, angle)
        dynamixel.write_and_get_response_multiple(ser, packet, id, verbose,
                                                  num_error_attempts)

        if verbose:
            print('Setting velocity for {0} to {1}...'.format(id, velocity))
        packet = packets.get_write_velocity_packet(servo_id, velocity)
        dynamixel.write_and_get_response_multiple(ser, packet, id, verbose,
                                                  num_error_attempts)

    if verbose:
        print('Sending action packet.')

    packet = packets.get_action_packet()
    ser.write(packet)