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)
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)
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)
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)