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)
