fprintf(stderr, "[ID:%03d] groupBulkWrite addparam failed", DXL2_ID)
quit()
# Bulkwrite goal position and LED value
dynamixel.groupBulkWriteTxPacket(groupwrite_num)
if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) != COMM_SUCCESS:
dynamixel.printTxRxResult(
PROTOCOL_VERSION,
dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION))
# Clear bulkwrite parameter storage
dynamixel.groupBulkWriteClearParam(groupwrite_num)
while 1:
# Bulkread present position and moving status
dynamixel.groupBulkReadTxRxPacket(groupread_num)
if dynamixel.getLastTxRxResult(port_num,
PROTOCOL_VERSION) != COMM_SUCCESS:
dynamixel.printTxRxResult(
PROTOCOL_VERSION,
dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION))
# Check if groupbulkread data of Dynamixel#1 is available
dxl_getdata_result = ctypes.c_ubyte(
dynamixel.groupBulkReadIsAvailable(groupread_num, DXL1_ID,
ADDR_PRO_PRESENT_POSITION,
LEN_PRO_PRESENT_POSITION)).value
if dxl_getdata_result != 1:
print("[ID:%03d] groupBulkRead getdata failed" % (DXL1_ID))
quit()
print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))
elif dxl_error != 0:
print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))
# Write Dynamixel#2 goal position
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_GOAL_POSITION, dxl_goal_position[index])
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))
while 1:
# Bulkread present position and moving status
dynamixel.groupBulkReadTxRxPacket(group_num)
dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)
if dxl_comm_result != COMM_SUCCESS:
print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))
# Check if groupbulkread data of Dynamixel#1 is available
dxl_getdata_result = ctypes.c_ubyte(dynamixel.groupBulkReadIsAvailable(group_num, DXL1_ID, ADDR_MX_PRESENT_POSITION, LEN_MX_PRESENT_POSITION)).value
if dxl_getdata_result != 1:
print("[ID:%03d] groupBulkRead getdata failed" % (DXL1_ID))
quit()
# Check if groupbulkread data of Dynamixel#2 is available
dxl_getdata_result = ctypes.c_ubyte(dynamixel.groupBulkReadIsAvailable(group_num, DXL2_ID, ADDR_MX_MOVING, LEN_MX_MOVING)).value
if dxl_getdata_result != 1:
print("[ID:%03d] groupBulkRead getdata failed" % (DXL2_ID))
quit()