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