def read_torque(ID):
res = dynamixel.read2ByteTxRx(port_num,PROTOCOL_VERSION,ID,ADDR_AX_TORQUE)
if dxl_comm_result != COMM_SUCCESS:
if constants.ENABLE_DXL_MESSAGES:
print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))
elif dxl_error != 0:
if constants.ENABLE_DXL_MESSAGES:
print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))
return res
def read_load(ID,prnt_enable):
#Read present load
dxl_present_load = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, ID, ADDR_AX_PRESENT_LOAD)
dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)
if dxl_comm_result != COMM_SUCCESS:
if constants.ENABLE_DXL_MESSAGES:
print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))
elif dxl_error != 0:
if constants.ENABLE_DXL_MESSAGES:
print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))
if prnt_enable==1:
# if constants.ENABLE_DXL_MESSAGES: print("[ID:%03d] PresLoad:%03d" % (ID, dxl_present_load))
pass
return dxl_present_load
def read_pos(ID,prnt_enable):
#Read present position
dxl_present_position = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, ID, ADDR_AX_PRESENT_POSITION)
dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)
if dxl_comm_result != COMM_SUCCESS:
if constants.ENABLE_DXL_MESSAGES:
print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))
elif dxl_error != 0:
if constants.ENABLE_DXL_MESSAGES:
print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))
if prnt_enable==1:
#if constants.ENABLE_DXL_MESSAGES: print("[ID:%03d] PresPos:%03d" % (ID, dxl_present_position))
if constants.ENABLE_DXL_MESSAGES:
print(dxl_present_position)
return dxl_present_position
return dxl_present_position