def get_mode(self, num_tries=RETRIES): mode = 0 retry = 0 while (retry < num_tries): cw_lim = dxl.read1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_CW_LIMIT) dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL) derror = dxl.getLastRxPacketError(self.port, PROTOCOL) ccw_lim = dxl.read1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_CCW_LIMIT) dcomm_result += dxl.getLastTxRxResult(self.port, PROTOCOL) derror = dxl.getLastRxPacketError(self.port, PROTOCOL) if dcomm_result != 0: print_v(dxl.getTxRxResult(PROTOCOL, dcomm_result)) retry += 1 elif derror != 0: print_v(dxl.getRxPacketError(PROTOCOL, derror)) retry += 1 else: if ((cw_lim == 0) & (ccw_lim == 0)): mode = 1 else: mode = 2 return mode print('[COMM_ERR] ' + str(get_caller())) return 0
def get_mode(self, num_tries=RETRIES): mode = 0 retry = 0 while (retry < num_tries): cw_lim = dxl.read1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_CW_LIMIT) dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL) derror = dxl.getLastRxPacketError(self.port, PROTOCOL) ccw_lim = dxl.read1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_CCW_LIMIT) dcomm_result += dxl.getLastTxRxResult(self.port, PROTOCOL) derror = dxl.getLastRxPacketError(self.port, PROTOCOL) if dcomm_result != 0: print_v(dxl.getTxRxResult(PROTOCOL, dcomm_result)) retry += 1 elif derror != 0: print_v(dxl.getRxPacketError(PROTOCOL, derror)) retry += 1 else: if ((cw_lim == 0) & (ccw_lim == 0)): mode = 1 else: mode = 2 return mode raise Exception("get_mode failed after {} tries".format(num_tries))
def get_fsr_readings(self, foot): id_dic = {'left': 111, 'right': 112} id = id_dic[foot] #fsr_reading = {'1':0, '2':0, '3':0, '4':0, 'x':0, 'y':0} fsr_reading = {} fsr_reg1 = { '1': ADDR_FSR_1, '2': ADDR_FSR_2, '3': ADDR_FSR_3, '4': ADDR_FSR_4 } fsr_reg2 = {'x': ADDR_FSR_X, 'y': ADDR_FSR_Y} for reg in fsr_reg1.keys(): fsr_reading[reg] = self.to_newton( dxl.read2ByteTxRx(self.port, self.protocol, id, fsr_reg1[reg])) self.check_result(id) self.check_error(id) for reg in fsr_reg2.keys(): fsr_reading[reg] = dxl.read1ByteTxRx(self.port, self.protocol, id, fsr_reg2[reg]) self.check_result(id) self.check_error(id) return fsr_reading
def readValue(self, address, size=1): if(size==1): v = dxl.read1ByteTxRx(self.socket, PROTOCOL_VERSION, self.servo_id, address) elif(size==2): v = dxl.read2ByteTxRx(self.socket, PROTOCOL_VERSION, self.servo_id, address) return v
def GetPresentVoltage(self, Id): try: present_voltaje = dynamixel.read1ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.PRESENT_VOLTAGE) if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0: dynamixel.printRxPacketError(Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION)) return present_voltaje except : print "Oops! Problem writing goal position..."
def get_voltage(self): voltage = dxl.read1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_PRESENT_VOLTAGE) dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL) derror = dxl.getLastRxPacketError(self.port, PROTOCOL) if dcomm_result != 0: print(dxl.getTxRxResult(PROTOCOL, dcomm_result)) elif derror != 0: print(dxl.getRxPacketError(PROTOCOL, derror)) return voltage/10.0
def get_temp(self): temp = dxl.read1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_PRESENT_TEMP) dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL) derror = dxl.getLastRxPacketError(self.port, PROTOCOL) if dcomm_result != 0: print(dxl.getTxRxResult(PROTOCOL, dcomm_result)) elif derror != 0: print(dxl.getRxPacketError(PROTOCOL, derror)) return temp
def readValue(self, address, size=1): if (size == 1): v = dxl.read1ByteTxRx(self.socket, PROTOCOL_VERSION, self.servo_id, address) elif (size == 2): v = dxl.read2ByteTxRx(self.socket, PROTOCOL_VERSION, self.servo_id, address) return v
def is_moving(self): moving = dxl.read1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_MOVING) dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL) derror = dxl.getLastRxPacketError(self.port, PROTOCOL) if dcomm_result != 0: print(dxl.getTxRxResult(PROTOCOL, dcomm_result)) elif derror != 0: print(dxl.getRxPacketError(PROTOCOL, derror)) return moving
def GetTorqueEnable(self, Id): try: torque_enable = dynamixel.read1ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.TORQUE_ENABLE) if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0: dynamixel.printRxPacketError(Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION)) return torque_enable except : print "Oops! Problem reading Torque..." return -3141592
def get_mode(self): mode = dxl.read1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_CONTROL_MODE) dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL) derror = dxl.getLastRxPacketError(self.port, PROTOCOL) if dcomm_result != 0: print(dxl.getTxRxResult(PROTOCOL, dcomm_result)) elif derror != 0: print(dxl.getRxPacketError(PROTOCOL, derror)) return mode
def get_error_status(self): error_status = dxl.read1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_HARDWARE_ERROR) dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL) derror = dxl.getLastRxPacketError(self.port, PROTOCOL) if dcomm_result != 0: print(dxl.getTxRxResult(PROTOCOL, dcomm_result)) elif derror != 0: print(dxl.getRxPacketError(PROTOCOL, derror)) return error_status
def GetTorqueEnable(self, Id): torque_enable = dynamixel.read1ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.TORQUE_ENABLE) if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0: dynamixel.printRxPacketError( Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION)) return torque_enable
def read_1(self, id, reg, port=0, doPrint=False): read_res = dynamixel.read1ByteTxRx(self.port_num[port], self.protocol, id, reg) dxl_comm_result = dynamixel.getLastTxRxResult(self.port_num[port], self.protocol) dxl_error = dynamixel.getLastRxPacketError(self.port_num[port], self.protocol) if dxl_comm_result != COMM_SUCCESS: print(dynamixel.getTxRxResult(self.protocol, dxl_comm_result)) elif dxl_error != 0: print(dynamixel.getRxPacketError(self.protocol, dxl_error)) if doPrint: print("[ID:%03d] Regist %03d: %03d" % (id, reg, read_res)) return read_res
def get_mode(self): mode=0 cw_lim = dxl.read1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_CW_LIMIT) dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL) derror = dxl.getLastRxPacketError(self.port, PROTOCOL) if dcomm_result != 0: print(dxl.getTxRxResult(PROTOCOL, dcomm_result)) elif derror != 0: print(dxl.getRxPacketError(PROTOCOL, derror)) ccw_lim = dxl.read1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_CCW_LIMIT) dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL) derror = dxl.getLastRxPacketError(self.port, PROTOCOL) if dcomm_result != 0: print(dxl.getTxRxResult(PROTOCOL, dcomm_result)) elif derror != 0: print(dxl.getRxPacketError(PROTOCOL, derror)) if((cw_lim == 0)&(ccw_lim == 0)): mode = 1 else: mode = 2 return mode
def GetPresentVoltage(self, Id): present_voltaje = dynamixel.read1ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.PRESENT_VOLTAGE) if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0: dynamixel.printRxPacketError( Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION)) return present_voltaje
def is_moving(self, num_tries = RETRIES): retry = 0 while(retry < num_tries): moving = dxl.read1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_MOVING) dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL) derror = dxl.getLastRxPacketError(self.port, PROTOCOL) if dcomm_result != 0: print_v(dxl.getTxRxResult(PROTOCOL, dcomm_result)) retry += 1 elif derror != 0: print_v(dxl.getRxPacketError(PROTOCOL, derror)) retry +=1 else: return moving
def GetPresentVoltage(self, Id): try: present_voltaje = dynamixel.read1ByteTxRx( self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.PRESENT_VOLTAGE) if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0: dynamixel.printRxPacketError( Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION)) return present_voltaje except: print "Oops! Problem writing goal position..."
def get_voltage(self, num_tries = RETRIES): retry = 0 while(retry < num_tries): voltage = dxl.read1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_PRESENT_VOLTAGE) dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL) derror = dxl.getLastRxPacketError(self.port, PROTOCOL) if dcomm_result != 0: print_v(dxl.getTxRxResult(PROTOCOL, dcomm_result)) retry +=1 elif derror != 0: print_v(dxl.getRxPacketError(PROTOCOL, derror)) retry +=1 else: return voltage/10.0
def get_temp(self, num_tries = RETRIES): retry = 0 while(retry < num_tries): temp = dxl.read1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_PRESENT_TEMP) dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL) derror = dxl.getLastRxPacketError(self.port, PROTOCOL) if dcomm_result != 0: print_v(dxl.getTxRxResult(PROTOCOL, dcomm_result)) retry +=1 elif derror != 0: print_v(dxl.getRxPacketError(PROTOCOL, derror)) retry +=1 else: return temp raise Exception("get_temp failed after {} tries".format(num_tries))
def GetTorqueEnable(self, Id): try: torque_enable = dynamixel.read1ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.TORQUE_ENABLE) if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0: dynamixel.printRxPacketError( Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION)) return torque_enable except: print "Oops! Problem reading Torque..." return -3141592
def get_temp(self, num_tries = RETRIES): retry = 0 while(retry < num_tries): temp = dxl.read1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_PRESENT_TEMP) dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL) derror = dxl.getLastRxPacketError(self.port, PROTOCOL) if dcomm_result != 0: print_v(dxl.getTxRxResult(PROTOCOL, dcomm_result)) retry +=1 elif derror != 0: print_v(dxl.getRxPacketError(PROTOCOL, derror)) retry +=1 else: return temp print('[COMM_ERR] ' + str(get_caller())) return None
def Read_Value(self, motorId, read_addr, read_len): # Read value dxl_result = -1 if(read_len == 4): dxl_result = dynamixel.read4ByteTxRx(self.port_num, self.proto_ver, motorId, read_addr) elif(read_len == 2): dxl_result = dynamixel.read2ByteTxRx(self.port_num, self.proto_ver, motorId, read_addr) elif(read_len == 1): dxl_result = dynamixel.read1ByteTxRx(self.port_num, self.proto_ver, motorId, read_addr) else: print('[ID:%03d]: invalid read length %d' %(motorId, read_len)) return dxl_result dxl_comm_result = dynamixel.getLastTxRxResult(self.port_num, self.proto_ver) dxl_error = dynamixel.getLastRxPacketError(self.port_num, self.proto_ver) if dxl_comm_result != COMM_SUCCESS: print(dynamixel.getTxRxResult(self.proto_ver, dxl_comm_result)) elif dxl_error != 0: print(dynamixel.getRxPacketError(self.proto_ver, dxl_error)) return dxl_result
# Wait for reset print("Wait for reset...") sleep(2) print("[ID:%03d] factoryReset Success!" % (DXL_ID)) # Set controller baudrate to dxl default baudrate if dynamixel.setBaudRate(port_num, FACTORYRST_DEFAULTBAUDRATE): print("Succeed to change the controller baudrate to : %d" % (FACTORYRST_DEFAULTBAUDRATE)) else: print("Failed to change the controller baudrate") getch() quit() # Read Dynamixel baudnum dxl_baudnum_read = dynamixel.read1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_BAUDRATE) dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) if dxl_comm_result != COMM_SUCCESS: print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) elif dxl_error != 0: print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) else: print("[ID:%03d] Dynamixel baudnum is now : %d" % (DXL_ID, dxl_baudnum_read)) # Write new baudnum dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_BAUDRATE, NEW_BAUDNUM) dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) if dxl_comm_result != COMM_SUCCESS: print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))
def __read_byte(self, servo_id, address): incoming = dynamixel.read1ByteTxRx(self.__port_num, PROTOCOL, servo_id, address) self.__verify_last_message() return incoming
print("Press any key to terminate...") getch() quit() # Seta a baud rate da porta if dynamixel.setBaudRate(port_num, BAUDRATE): print("Succeeded to change the baudrate!") else: print("Failed to change the baudrate!") print("Press any key to terminate...") getch() quit() print("Dynamixel ID[%s] info" % DXL_ID) MOVING = dynamixel.read1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MOVING) if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) != COMM_SUCCESS: dynamixel.printTxRxResult( PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) != 0: dynamixel.printRxPacketError( PROTOCOL_VERSION, dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) else: if MOVING == 1: print("The Dynamixel is moving") else: print("Dynamixel is stopped") voltage = dynamixel.read1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID,
# Wait for reset print("Wait for reset...") sleep(2) print("[ID:%03d] factoryReset Success!" % (DXL_ID)) # Set controller baudrate to dxl default baudrate if dynamixel.setBaudRate(port_num, FACTORYRST_DEFAULTBAUDRATE): print("Succeed to change the controller baudrate to : %d" % (FACTORYRST_DEFAULTBAUDRATE)) else: print("Failed to change the controller baudrate") getch() quit() # Read Dynamixel baudnum dxl_baudnum_read = dynamixel.read1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_BAUDRATE) if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) != COMM_SUCCESS: dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) != 0: dynamixel.printRxPacketError(PROTOCOL_VERSION, dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) else: print("[ID:%03d] Dynamixel baudnum is now : %d" % (DXL_ID, dxl_baudnum_read)) # Write new baudnum dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_BAUDRATE, NEW_BAUDNUM) if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) != COMM_SUCCESS: dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) != 0: dynamixel.printRxPacketError(PROTOCOL_VERSION, dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) else: print("[ID:%03d] Set Dynamixel baudnum to : %d" % (DXL_ID, NEW_BAUDNUM))