def set_mode(self,mode, num_tries = 5): # 1 for wheel, 2 for joint if((mode != 1) & (mode != 2)): print_v('Error: Mode must be 1 or 2') return -1 retry = 0 while(retry < num_tries): if(mode == 1): dxl.write1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_CW_LIMIT, 0) dxl.write1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_CCW_LIMIT, 0) if(mode == 2): dxl.write1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_CW_LIMIT, 0) dxl.write1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_CCW_LIMIT, 4095) 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: self.mode = mode return 0 print('[COMM_ERR] ' + str(get_caller())) return -1
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 print('[COMM_ERR] ' + str(get_caller())) return None
def get_position(self, num_tries = RETRIES): retry = 0 while(retry < num_tries): pos = dxl.read2ByteTxRx(self.port, PROTOCOL, self.id, ADDR_PRESENT_POSITION) 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 self.value2rad(pos) print('[COMM_ERR] ' + str(get_caller())) return None
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 print('[COMM_ERR] ' + str(get_caller())) return None
def set_torque_limit(self, torque, num_tries = RETRIES): retry = 0 while(retry < num_tries): dxl.write2ByteTxRx(self.port, PROTOCOL, self.id, ADDR_TORQUE_LIMIT, int(torque * 1023)) 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 0 print('[COMM_ERR] ' + str(get_caller())) return -1
def disable_torque(self, num_tries = RETRIES): retry = 0 while(retry < num_tries): dxl.write1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_TORQUE_ENABLE, 0) 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 0 print('[COMM_ERR] ' + str(get_caller())) return -1
def set_position(self, pos, num_tries = RETRIES): value = self.rad2value(pos) retry = 0 while(retry < num_tries): dxl.write2ByteTxRx(self.port, PROTOCOL, self.id, ADDR_GOAL_POSITION, value) 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 0 print('[COMM_ERR] ' + str(get_caller())) return -1
def get_load(self, num_tries = RETRIES): # returns normalized load retry = 0 while(retry < num_tries): load = dxl.read2ByteTxRx(self.port, PROTOCOL, self.id, ADDR_PRESENT_LOAD) 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(load > 1024): return (load & 1023)/1023.0 else: return -(load & 1023)/1023.0 print('[COMM_ERR] ' + str(get_caller())) return None
def get_speed(self, num_tries = RETRIES): # returns normalized speed. multiply by max speed to get radians/s retry = 0 while(retry < num_tries): speed = dxl.read2ByteTxRx(self.port, PROTOCOL, self.id, ADDR_PRESENT_SPEED) 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(speed > 1024): return (speed & 1023)/1023.0 else: return -(speed & 1023)/1023.0 print('[COMM_ERR] ' + str(get_caller())) return None
def set_speed(self, speed, num_tries = RETRIES): if(self.mode == 2): value = int(1023*abs(speed)) elif(self.mode == 1): value = int(1023*abs(speed)) if(speed>0.0): value = value + 1024 retry = 0 while(retry < num_tries): dxl.write2ByteTxRx(self.port, PROTOCOL, self.id, ADDR_MOVING_SPEED, value) 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 0 print('[COMM_ERR] ' + str(get_caller())) return -1
def set_gains(self,kp,ki,kd, num_tries = RETRIES): retry = 0 while(retry < num_tries): dxl.write1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_P_GAIN, kp) dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL) derror = dxl.getLastRxPacketError(self.port, PROTOCOL) dxl.write1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_I_GAIN, ki) dcomm_result += dxl.getLastTxRxResult(self.port, PROTOCOL) derror += dxl.getLastRxPacketError(self.port, PROTOCOL) dxl.write1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_D_GAIN, kd) 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 0 print('[COMM_ERR] ' + str(get_caller())) return -1
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 -1