def Init_Param_Storage(self):
groupread_num = self.groupread_num
# Add parameter storage for Dynamixel#1 current
dxl_addparam_result = ctypes.c_ubyte(
dynamixel.groupSyncReadAddParam(groupread_num, self.m1id)).value
if dxl_addparam_result != 1:
print("[ID:%03d] groupSyncRead addparam failed" % (self.m1id))
quit()
# Add parameter storage for Dynamixel#2 current
dxl_addparam_result = ctypes.c_ubyte(
dynamixel.groupSyncReadAddParam(groupread_num, self.m2id)).value
if dxl_addparam_result != 1:
print("[ID:%03d] groupSyncRead addparam failed" % (self.m2id))
quit()
# Add parameter storage for Dynamixel#2 current
dxl_addparam_result = ctypes.c_ubyte(
dynamixel.groupSyncReadAddParam(groupread_num, self.m3id)).value
if dxl_addparam_result != 1:
print("[ID:%03d] groupSyncRead addparam failed" % (self.m3id))
quit()
# Add parameter storage for Dynamixel#4 current
dxl_addparam_result = ctypes.c_ubyte(
dynamixel.groupSyncReadAddParam(groupread_num, self.m4id)).value
if dxl_addparam_result != 1:
print("[ID:%03d] groupSyncRead addparam failed" % (self.m4id))
quit()
def sync_read(self, ids, reg, length, port=0, doPrint=False):
groupread_num = dynamixel.groupSyncRead(self.port_num[port], self.protocol, reg, length)
for dxl_id in ids:
# Add parameter storage for Dynamixel#1 present position value
dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncReadAddParam(groupread_num, dxl_id)).value
if dxl_addparam_result != 1:
print("[ID:%03d] groupSyncRead addparam failed" % (dxl_id))
return False
# Syncread present position
dynamixel.groupSyncReadTxRxPacket(groupread_num)
dxl_comm_result = dynamixel.getLastTxRxResult(self.port_num[port], self.protocol)
if dxl_comm_result != COMM_SUCCESS:
print(dynamixel.getTxRxResult(self.protocol, dxl_comm_result))
for dxl_id in ids:
# Check if groupsyncread data of Dynamixels is available
dxl_getdata_result = ctypes.c_ubyte(dynamixel.groupSyncReadIsAvailable(groupread_num, dxl_id, reg, length)).value
if dxl_getdata_result != 1:
print("[ID:%03d] groupSyncRead getdata failed" % (dxl_id))
return False
for dxl_id in ids:
dxl_result = dynamixel.groupSyncReadGetData(groupread_num, dxl_id, reg, length)
print("[ID:%03d] result is %d" % (dxl_id, dxl_result))
return True
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))
# Enable position p gain #7
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL7_ID, ADDR_XM430_POSITION_P_GAIN, dxl_position_p_gain)
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))
# Add parameter storage for Dynamixel#1 present position value
dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncReadAddParam(groupread_num, DXL1_ID)).value
if dxl_addparam_result != 1:
print("[ID:%03d] groupSyncRead addparam failed" % (DXL1_ID))
quit()
# Add parameter storage for Dynamixel#2 present position value
dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncReadAddParam(groupread_num, DXL2_ID)).value
if dxl_addparam_result != 1:
print("[ID:%03d] groupSyncRead addparam failed" % (DXL2_ID))
quit()
# Add parameter storage for Dynamixel#3 present position value
dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncReadAddParam(groupread_num, DXL3_ID)).value
if dxl_addparam_result != 1:
print("[ID:%03d] groupSyncRead addparam failed" % (DXL3_ID))
quit()
# Enable Dynamixel#2 Torque
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID,
ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE)
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("Dynamixel#2 has been successfully connected")
# Add parameter storage for Dynamixel#1 present position value
dxl_addparam_result = ctypes.c_ubyte(
dynamixel.groupSyncReadAddParam(groupread_num, DXL1_ID)).value
if dxl_addparam_result != 1:
print("[ID:%03d] groupSyncRead addparam failed" % (DXL1_ID))
quit()
# Add parameter storage for Dynamixel#2 present position value
dxl_addparam_result = ctypes.c_ubyte(
dynamixel.groupSyncReadAddParam(groupread_num, DXL2_ID)).value
if dxl_addparam_result != 1:
print("[ID:%03d] groupSyncRead addparam failed" % (DXL2_ID))
quit()
while 1:
print("Press any key to continue! (or press ESC to quit!)")
if getch() == chr(ESC_ASCII_VALUE):
break
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))
# Enable Dynamixel Torque
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE)
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))
# Add parameter storage for Dynamixel present position value
dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncReadAddParam(groupread_num, DXL_ID)).value
if dxl_addparam_result != 1:
print("[ID:%03d] groupSyncRead addparam failed" % (DXL_ID))
quit()
while 1:
print("Press any key to continue! (or press ESC to quit!)")
if getch() == chr(ESC_ASCII_VALUE):
break
# Add Dynamixel#1 goal position value to the Syncwrite storage
dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncWriteAddParam(groupwrite_num, DXL_ID, dxl_goal_position[index], LEN_PRO_GOAL_POSITION)).value
print(dxl_addparam_result)
if dxl_addparam_result != 1:
print("[ID:%03d] groupSyncWrite addparam failed" % (DXL_ID))
else:
print("Dynamixel#1 has been successfully connected")
# Enable Dynamixel#2 Torque
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE)
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("Dynamixel#2 has been successfully connected")
# Add parameter storage for Dynamixel#1 present position value
dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncReadAddParam(groupread_num, DXL1_ID)).value
if dxl_addparam_result != 1:
print("[ID:%03d] groupSyncRead addparam failed" % (DXL1_ID))
quit()
# Add parameter storage for Dynamixel#2 present position value
dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncReadAddParam(groupread_num, DXL2_ID)).value
if dxl_addparam_result != 1:
print("[ID:%03d] groupSyncRead addparam failed" % (DXL2_ID))
quit()
while 1:
print("Press any key to continue! (or press ESC to quit!)")
if getch() == chr(ESC_ASCII_VALUE):
break
