def ping(self, id, port=0, doPrint=False):
dxl_model_number = dynamixel.pingGetModelNum(self.port_num[port], self.protocol, id)
dxl_comm_result = dynamixel.getLastTxRxResult(self.port_num[port], self.protocol)
dxl_error = dynamixel.getLastRxPacketError(self.port_num[port], self.protocol)
if dxl_comm_result != COMM_SUCCESS:
if doPrint:
print(dynamixel.getTxRxResult(self.protocol, dxl_comm_result))
return False
elif dxl_error != 0:
if doPrint:
print(dynamixel.getRxPacketError(self.protocol, dxl_error))
return False
else:
if doPrint:
print("[ID:%03d] ping Succeeded. Dynamixel model number : %d" % (id, dxl_model_number))
return True
def search(id_search_min, id_search_max, baudrates_search_list):
"""Search for servos in range of ``id_search_min`` and ``id_search_max`` for all baudrates in ``baudrates_search_list``.
:param int id_search_min: ID to start searching.
:param int id_search_max: ID to stop pinging.
:param list baudrates_search_list: List containing the baudrates that the user want to search.
:return: ``found_servos`` list containing the servos found.
:rtype: List containing the ``Dynamixel`` object servos"""
#SEARCHING IN THE NETWORK
# 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
#Declaring the limits of the search
init = id_search_min
end = id_search_max
#List containing the found servos in the network
found_servos = []
#Tries to ping in protocols 1 and 2
for protocol in PROTOCOL_VERSIONS:
print("Using protocol %s" % str(protocol))
#Loop through all baudrates
for baudrate in baudrates_search_list:
actual_id = init
# 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
time.sleep(0.2)
while actual_id <= end:
print("Pinging in ID: %s " % actual_id)
# Try to ping the Dynamixel
# Get Dynamixel model number
dxl_model_number = dynamixel.pingGetModelNum(
port_num, protocol, actual_id)
dxl_comm_result = dynamixel.getLastTxRxResult(
port_num, protocol)
dxl_error = dynamixel.getLastRxPacketError(port_num, protocol)
if dxl_comm_result != COMM_SUCCESS:
print(dynamixel.getTxRxResult(protocol, dxl_comm_result))
elif dxl_error != 0:
print(dynamixel.getRxPacketError(protocol, dxl_error))
else:
print(
"[ID:%03d] ping Succeeded. Dynamixel model number : %d"
% (actual_id, dxl_model_number))
servo = Dynamixel()
servo.id = actual_id
servo.baudrate = baudrate
servo.protocol = protocol
servo.model = dxl_model_number
found_servos.append(servo)
actual_id = actual_id + 1
# Close port
dynamixel.closePort(port_num)
return found_servos
if dynamixel.openPort(port_num):
print("Succeeded to open the port!")
else:
print("Failed to open the port!")
print("Press any key to terminate...")
getch()
quit()
# 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()
# Try to ping the Dynamixel
# Get Dynamixel model number
dxl_model_number = dynamixel.pingGetModelNum(port_num, PROTOCOL_VERSION, DXL_ID)
if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) != COMM_SUCCESS:
dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION))
elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(PROTOCOL_VERSION, dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION))
print("[ID:%03d] ping Succeeded. Dynamixel model number : %d" % (DXL_ID, dxl_model_number))
# Close port
dynamixel.closePort(port_num)
print("Failed to open the port!")
print("Press any key to terminate...")
getch()
quit()
# 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()
# Try to ping the Dynamixel
# Get Dynamixel model number
dxl_model_number = dynamixel.pingGetModelNum(port_num, PROTOCOL_VERSION,
DXL_ID)
dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)
if dxl_comm_result != COMM_SUCCESS:
print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))
elif dxl_error != 0:
print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))
else:
print("[ID:%03d] ping Succeeded. Dynamixel model number : %d" %
(DXL_ID, dxl_model_number))
# Close port
dynamixel.closePort(port_num)
