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))