def factory_reset(id, baudrate):
"""Resets a servo to factory config.
:param int id: Servo ``ìd``
:param baudrate: Baudrate of the servo to be configured
:return:
--``PORT_ERROR`` case it fails to open the port.
--``BAUDRATE_ERROR`` case it fails to change baudrate.
--``COMM_ERROR`` case there is a communication error.
--``HARDWARE_COMM_ERROR`` case there is a hardware communication error.
--``NONE`` case the operation succeeds."""
# Get methods and members of PortHandlerLinux or PortHandlerWindows
port_num = dynamixel.portHandler(DEVICENAME)
# Initialize PacketHandler Structs
dynamixel.packetHandler()
# Open port
if dynamixel.openPort(port_num):
print("Succeeded to open the port!")
else:
print("Failed to open the port!")
return PORT_ERROR
# Set port baudrate
if dynamixel.setBaudRate(port_num, baudrate):
print("Succeeded to change the baudrate!")
else:
print("Failed to change the baudrate!")
return BAUDRATE_ERROR
dynamixel.factoryReset(port_num, PROTOCOL_1, id, OPERATION_MODE)
if dynamixel.getLastTxRxResult(port_num, PROTOCOL_1) != COMM_SUCCESS:
print("Aborted")
dynamixel.printTxRxResult(
PROTOCOL_1, dynamixel.getLastTxRxResult(port_num, PROTOCOL_1))
return COMM_ERROR
elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_1) != 0:
dynamixel.printRxPacketError(
PROTOCOL_1, dynamixel.getLastRxPacketError(port_num, PROTOCOL_1))
return HARDWARE_COMM_ERROR
# Wait for reset
time.sleep(2)
# Set port baudrate
if dynamixel.setBaudRate(port_num, BAUDRATE):
print("Succeeded to change the baudrate!")
else:
print("Failed to change the baudrate!")
print("Press any key to terminate...")
getch()
quit()
# Read present baudrate of the controller
print("Now the controller baudrate is : %d" % (dynamixel.getBaudRate(port_num)))
# Try factoryreset
print("[ID:%03d] Try factoryreset : " % (DXL_ID))
dynamixel.factoryReset(port_num, PROTOCOL_VERSION, DXL_ID, OPERATION_MODE)
dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)
if dxl_comm_result != COMM_SUCCESS:
print("Aborted")
print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))
elif dxl_error != 0:
print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))
# Wait for reset
print("Wait for reset...")
sleep(2)
print("[ID:%03d] factoryReset Success!" % (DXL_ID))
# Set port baudrate
if dynamixel.setBaudRate(port_num, BAUDRATE):
print("Succeeded to change the baudrate!")
else:
print("Failed to change the baudrate!")
print("Press any key to terminate...")
getch()
quit()
# Read present baudrate of the controller
print("Now the controller baudrate is : %d" % (dynamixel.getBaudRate(port_num)))
# Try factoryreset
print("[ID:%03d] Try factoryreset : " % (DXL_ID))
dynamixel.factoryReset(port_num, PROTOCOL_VERSION, DXL_ID, OPERATION_MODE)
if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) != COMM_SUCCESS:
print("Aborted")
dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION))
quit()
elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(PROTOCOL_VERSION, dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION))
# Wait for reset
print("Wait for reset...")
sleep(2)
print("[ID:%03d] factoryReset Success!" % (DXL_ID))
# Set controller baudrate to dxl default baudrate
