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 set_acc(ID,acce):
#Write goal acce
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, ID, ADDR_AX_GOAL_ACCE, acce)
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))
def write_pos(ID, final_pos):
# Write goal position
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, ID, ADDR_AX_GOAL_POSITION, final_pos)
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))
def enable_bot(ID):
# Enable Dynamixel Torque
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, ID, ADDR_AX_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:
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))
else:
if constants.ENABLE_DXL_MESSAGES:
print("[] Dynamixel has been successfully connected")
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
