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()
# Get Dynamixel#1 present position value
dxl1_present_position = dynamixel.groupBulkReadGetData(group_num, DXL1_ID, ADDR_MX_PRESENT_POSITION, LEN_MX_PRESENT_POSITION)
# Get Dynamixel#2 moving status value
dxl2_moving = dynamixel.groupBulkReadGetData(group_num, DXL2_ID, ADDR_MX_MOVING, LEN_MX_MOVING)
print("[ID:%03d] Present Position : %d \t [ID:%03d] Is Moving : %d" % (DXL1_ID, dxl1_present_position, DXL2_ID, dxl2_moving))
if not (abs(dxl_goal_position[index] - dxl1_present_position) > DXL_MOVING_STATUS_THRESHOLD):
break
# Change goal position
if index == 0:
index = 1
else:
index = 0
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(groupread_num, DXL2_ID,
ADDR_PRO_LED_RED,
LEN_PRO_LED_RED)).value
if dxl_getdata_result != 1:
print("[ID:%03d] groupBulkRead getdata failed" % (DXL2_ID))
quit()
# Get Dynamixel#1 present position value
dxl1_present_position = dynamixel.groupBulkReadGetData(
groupread_num, DXL1_ID, ADDR_PRO_PRESENT_POSITION,
LEN_PRO_PRESENT_POSITION)
# Get Dynamixel#2 moving status value
dxl2_led_value_read = dynamixel.groupBulkReadGetData(
groupread_num, DXL2_ID, ADDR_PRO_LED_RED, LEN_PRO_LED_RED)
print("[ID:%03d] Present Position : %d \t [ID:%03d] LED Value: %d" %
(DXL1_ID, dxl1_present_position, DXL2_ID, dxl2_led_value_read))
if not (abs(dxl_goal_position[index] - dxl1_present_position) >
DXL_MOVING_STATUS_THRESHOLD):
break
# Change goal position
if index == 0:
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(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()
# Check if groupbulkread data of Dynamixel#2 is available
dxl_getdata_result = ctypes.c_ubyte(dynamixel.groupBulkReadIsAvailable(groupread_num, DXL2_ID, ADDR_PRO_LED_RED, LEN_PRO_LED_RED)).value
if dxl_getdata_result != 1:
print("[ID:%03d] groupBulkRead getdata failed" % (DXL2_ID))
quit()
# Get Dynamixel#1 present position value
dxl1_present_position = dynamixel.groupBulkReadGetData(groupread_num, DXL1_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION)
# Get Dynamixel#2 moving status value
dxl2_led_value_read = dynamixel.groupBulkReadGetData(groupread_num, DXL2_ID, ADDR_PRO_LED_RED, LEN_PRO_LED_RED)
print("[ID:%03d] Present Position : %d \t [ID:%03d] LED Value: %d" % (DXL1_ID, dxl1_present_position, DXL2_ID, dxl2_led_value_read))
if not (abs(dxl_goal_position[index] - dxl1_present_position) > DXL_MOVING_STATUS_THRESHOLD):
break
# Change goal position
if index == 0:
index = 1
else:
index = 0
