def multi_set_status_return(ser,
servo_id,
reg_value,
t_init_sleep=0.1,
t_sleep=0.05):
"""
Set status_return mode of the servos.
:param ser: The ``serial`` port to use.
:param servo_id: the list of servo IDs.
:param reg_value: value to write in STATUS_RETURN_LEVEL register (cf registers.py).
:param t_init_sleep: pause time before start change reg values.
:param t_sleep: pause time between settings in each servo.
If status_return disable => return nothing when new set
If status_return enable => return status packet when new set
"""
n_servo = len(servo_id)
reg_addr = registers.STATUS_RETURN_LEVEL
if (reg_value == registers.STATUS_RETURN.RETURN_FOR_ALL_PACKETS):
wait_response = False
elif (reg_value == registers.STATUS_RETURN.RETURN_ONLY_FOR_READ):
wait_response = True
else:
raise ValueError("Incorrect value for reg_value")
sleep(t_init_sleep)
for n in range(n_servo):
dynamixel.set_reg_1b(ser, servo_id[n], reg_addr, reg_value,
wait_response)
#print('Reg @{} set successfully at {} !'.format(reg_addr, reg_value) )
sleep(t_sleep)
########################################################################################
#packet_bytes = [ servo_id, 0x02, 0x01 ]
# registre value to set (cf DOC)
reg_value = 512
# registre address to set (cf registers.py ou la DOC juste au-dessus)
reg_addr = registers. # /!\ /!\ WARNING marche uniquement sur 1 bytes !!!
# goal position est sur 2 bytes !!!!
# try:
ser = dynamixel.get_serial_for_url(serial_port)
# dynamixel.send_action_packet( ser )
# packet = packets.get_packet(packet_bytes)
# dynamixel.write_and_no_get_response(ser , packet)
# dynamixel.get_position( ser )
dynamixel.set_reg_1b(ser, servo_id, reg_addr, reg_value)
print('Reg set successfully at {} !'.format(reg_value) )
# except Exception as e:
# print('Unable to set ID.')
# print(e)
print "success"
move = -1
offset = 512 + turnOffset
n_servo = len(servo_id)
# pré-calculs
mvt_speed = 2 * pi * 1. / resolution
amplitude_norm = amplitude * (float(n_period) / n_servo) * 1024. / 300.
omega = 2 * pi * n_period / float(n_servo)
# enable write response
for n in range(n_servo):
reg_value = registers.STATUS_RETURN.RETURN_FOR_ALL_PACKETS
reg_addr = registers.STATUS_RETURN_LEVEL # /!\ /!\ WARNING marche uniquement sur 1 bytes !!!
# goal position est sur 2 bytes !!!!
dynamixel.set_reg_1b(ser, servo_id[n], reg_addr, reg_value,
False) # revoie pas de reponse
print('Reg @{} set successfully at {} !'.format(reg_addr, reg_value))
### Velocity settings ###
v = 400
velocity = [v for i in range(n_servo)]
sleep(0.1)
for n in range(n_servo):
dynamixel.set_velocity(ser, servo_id[n], velocity[n])
#sleep(0.1)
# disable write response
for n in range(n_servo):
reg_value = registers.STATUS_RETURN.RETURN_ONLY_FOR_READ
reg_addr = registers.STATUS_RETURN_LEVEL # /!\ /!\ WARNING marche uniquement sur 1 bytes !!!
# goal position est sur 2 bytes !!!!