import pypot.dynamixel

ports = pypot.dynamixel.get_available_ports()
if not ports:
    raise IOError('no port found!')
print 'ports found', ports

"""
Explanation: Setup a new motor
This is an alternative to the use of the Herborist software
Plug only one new motor at a time
First scan all available ports.
End of explanation
"""


my_port = "/dev/ttyACM1" #Change this value to match your setup
using_XL320 = False      #Change this value to True if you use XL-320 motors

"""
Explanation: From the available ports, select the port where the new motor is pluggued.
If the new motor is a XL-320 (using another protocol), set the using_XL320 flag to True.
End of explanation
"""


old_id = 1             #Should be 1 if the motor has never been configured
new_id = 10            #Change this value

old_baudrate = 57600   #Should be 57600 for new MX-28 or MX-64, 1000000 for new AX612A or XL-320
new_baudrate = 1000000 #Should be 1000000

"""
Explanation: Select the new ID and the new baudrate you wish for your motor.
End of explanation
"""


if using_XL320:
    dxl_io = pypot.dynamixel.Dxl320IO(my_port, baudrate=old_baudrate)
else:
    dxl_io = pypot.dynamixel.DxlIO(my_port, baudrate=old_baudrate)
        
print "scanning"
found =  dxl_io.scan(range(60))
print found

if old_id in found:
    dxl_io.set_return_delay_time({old_id : 0})
    dxl_io.change_id({old_id : new_id})
    dxl_io.change_baudrate({new_id : new_baudrate})
else:
    print "no motor found with ID ",old_id," at baudrate ",old_baudrate, " on port ",my_port

dxl_io.close()

"""
Explanation: Change the ID, baudrate and set the return time to 0.
End of explanation
"""


dxl_io = pypot.dynamixel.DxlIO(my_port, baudrate=new_baudrate)
print "scanning"
found =  dxl_io.scan(range(60))
print found
dxl_io.close()

"""
Explanation: Check that the new motor is detected with its new id and new baudrate.
End of explanation
"""


