def printData(): # Read Dynamixel Data # read1ByteTxRx return byte # read2ByteTxRx return int global t1 global velocity tmpTime = timedelta(0, time.clock()-t1) diff = (tmpTime.days * 24 * 60 * 60 + tmpTime.seconds) * 1000 + tmpTime.microseconds / 1000.0 if diff > 0.98: # each 1 ms = 0.98, each 10 ms = 9.98 dxl_present_position = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_PRESENT_POSITION) dxl_present_speed = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_PRESENT_SPEED) dxl_present_load = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_PRESENT_LOAD) tmp_temperature = dynamixel.readTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_PRESENT_TEMPERATURE, 1) dxl_present_temperature = dynamixel.getDataRead(port_num, PROTOCOL_VERSION, 1, 0) tmp_voltage = dynamixel.readTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_PRESENT_VOLTAGE, 1) dxl_present_voltage = dynamixel.getDataRead(port_num, PROTOCOL_VERSION, 1, 0) dxl_current = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_CURRENT) newTime = timedelta(0, time.clock()-t0) end = round((newTime.days * 24 * 60 * 60 + newTime.seconds) * 1000 + newTime.microseconds / 1000.0,1) t1 = time.clock() # 4095 steps on 280.6 degree = 0,06852258852258852258852258852259 dxl_angle = dxl_present_position * 0.06852258852258852258852258852259 steer_angle = (dxl_angle/62) * 30 # 0 - 1023 speed = dxl_present_speed * 0.111 # in rpm (max 627 = 69.9 rpm) load = dxl_present_load # 0-1023 CCW , 1024 - 2048 CW temperature = dxl_present_temperature voltage = dxl_present_voltage / 10.0 current = dxl_current torque = 0 csvwriterUnits.writerow([end,steer_angle,speed,load,temperature,voltage,current,int(str(velocity).encode('hex'), 16)]) csvwriterRaw.writerow([end,dxl_present_position,dxl_present_speed,dxl_present_load,dxl_present_temperature,dxl_present_voltage,dxl_current,int(str(velocity).encode('hex'), 16)])
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 GetGoalPosition(self, Id): try: goal_position = dynamixel.read2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.GOAL_POSITION) if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0: dynamixel.printRxPacketError(Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION)) return goal_position except : print "Oops! Problem reading..."
def GetTorqueLimit(self, Id): try: torque_limit = dynamixel.read2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.TORQUE_LIMIT) 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_limit except : print "Oops! Problem reading Torque limit..."
def GetMovingSpeed(self, Id): try: moving_speed = dynamixel.read2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.MOVING_SPEED) if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0: dynamixel.printRxPacketError(Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION)) return moving_speed except : print "Oops! Problem reading moving speed..."
def GetCCWAngleLimit(self, Id): try: ccw_angle_limit = dynamixel.read2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.CCW_ANGLE_LIMIT) if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0: dynamixel.printRxPacketError(Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION)) return ccw_angle_limit except : print "Oops! Problem reading..."
def GetPresentLoad(self, Id): try: present_load = dynamixel.read2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.PRESENT_LOAD) 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_load except : print "Oops! Problem writing goal position..."
def GetMaxTorque(self, Id, maxTorque): try: max_torque = dynamixel.read2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.MAX_TORQUE) if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0: dynamixel.printRxPacketError(Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION)) return max_torque except : print "Oops! Problem reading max torque..."
def get_present_position(self, ids): positions = [] for id in ids: present_position = dxl.read2ByteTxRx(self.port, self.protocol, id, ADDR_PRES_POS) # if not self.check_result() and not self.check_error(): present_position = self.to_degree(present_position) - 180 positions.append(present_position) return dict(zip(ids, positions))
def get_position(self): 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(dxl.getTxRxResult(PROTOCOL, dcomm_result)) elif derror != 0: print(dxl.getRxPacketError(PROTOCOL, derror)) return self.value2rad(pos)
def _read(self, DXL_ID): dxl_present_position = dynamixel.read2ByteTxRx(self.portHandler, self.PROTOCOL_VERSION, DXL_ID, self.ADDR_MX_PRESENT_POSITION) dxl_comm_result = dynamixel.getLastTxRxResult(self.portHandler, self.PROTOCOL_VERSION) dxl_error = dynamixel.getLastRxPacketError(self.portHandler, self.PROTOCOL_VERSION) if(dxl_comm_result != COMM_SUCCESS): print(dynamixel.getTxRxResult(self.PROTOCOL_VERSION, self.dxl_comm_result)) elif(dxl_error != 0): print(dynamixel.getRxPacketError(self.PROTOCOL_VERSION, dxl_error)) return (dxl_present_position - 2048) * 0.088
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 get_load(self): 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(dxl.getTxRxResult(PROTOCOL, dcomm_result)) elif derror != 0: print(dxl.getRxPacketError(PROTOCOL, derror)) return (load/1023.0)-1.0
def GetPresentPosition(self, Id): try: present_position = dynamixel.read2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.PRESENT_POSITION) if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0: dynamixel.printRxPacketError(Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION)) else: return present_position except : print "Oops! Problem writing goal position..." return -3141592
def receiveCurrAngle(self): ''' Reads the current position of this servomotor alone. The read position is stored and can be accessed via method getAngle() ''' self.currValue = dxl.read2ByteTxRx(self.socket, PROTOCOL_VERSION, self.servo_id, \ ADDR_MX_PRESENT_POSITION) - self.centerValue self.currAngle = pi * float(self.currValue) / 2048.0 return self.currAngle
def GetGoalPosition(self, Id): goal_position = dynamixel.read2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.GOAL_POSITION) if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0: dynamixel.printRxPacketError( Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION)) return goal_position
def GetMovingSpeed(self, Id): moving_speed = dynamixel.read2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.MOVING_SPEED) if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0: dynamixel.printRxPacketError( Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION)) return moving_speed
def GetMaxTorque(self, Id, maxTorque): max_torque = dynamixel.read2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.MAX_TORQUE) if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0: dynamixel.printRxPacketError( Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION)) return max_torque
def GetTorqueLimit(self, Id): torque_limit = dynamixel.read2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.TORQUE_LIMIT) 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_limit
def GetPresentLoad(self, Id): present_load = dynamixel.read2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.PRESENT_LOAD) 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_load
def GetCWAngleLimit(self, Id): cw_angle_limit = dynamixel.read2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.CW_ANGLE_LIMIT) if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0: dynamixel.printRxPacketError( Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION)) return cw_angle_limit
def receiveCurrAngle(self): ''' Reads the current position of this servomotor alone. The read position is stored and can be accessed via method getAngle() ''' self.currValue = dxl.read2ByteTxRx(self.socket, PROTOCOL_VERSION, self.servo_id, \ ADDR_MX_PRESENT_POSITION) - self.centerValue self.currAngle = pi*float(self.currValue)/2048.0 return self.currAngle
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)
def GetMaxTorque(self, Id, maxTorque): try: max_torque = dynamixel.read2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.MAX_TORQUE) if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0: dynamixel.printRxPacketError( Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION)) return max_torque except: print "Oops! Problem reading max torque..."
def GetPresentLoad(self, Id): try: present_load = dynamixel.read2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.PRESENT_LOAD) 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_load except: print "Oops! Problem writing goal position..."
def GetCCWAngleLimit(self, Id): try: ccw_angle_limit = dynamixel.read2ByteTxRx( self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.CCW_ANGLE_LIMIT) if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0: dynamixel.printRxPacketError( Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION)) return ccw_angle_limit except: print "Oops! Problem reading..."
def GetPresentPosition(self, Id): # Read present position present_position = dynamixel.read2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.PRESENT_POSITION) if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0: dynamixel.printRxPacketError( Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION)) else: return present_position
def GetPresentPosition(self, Id): try: present_position = dynamixel.read2ByteTxRx( self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.PRESENT_POSITION) if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0: dynamixel.printRxPacketError( Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION)) else: return present_position except: print "Oops! Problem writing goal position..." return -3141592
def get_load(self, num_tries=RETRIES): 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: return (load / 1023.0) - 1.0 raise Exception("get_load failed after {} tries".format(num_tries))
def PresentPos_1(self): #Print current pos without goalpos global DXL_MOVING_STATUS_THRESHOLD #Read present position self.dxl_present_position = dynamixel.read2ByteTxRx( self.port_num, self.pro_ver, self.ID, self.addr_present) self.dxl_comm_result = dynamixel.getLastTxRxResult( self.port_num, self.pro_ver) self.dxl_error = dynamixel.getLastRxPacketError( self.port_num, self.pro_ver) if self.dxl_comm_result != self.COMM_SUCCESS: return (dynamixel.getTxRxResult(self.pro_ver, self.dxl_comm_result)) elif self.dxl_error != 0: return (dynamixel.getRxPacketError(self.pro_ver, self.dxl_error)) print("[ID:%03d] PresPos:%03d" % (self.ID, self.dxl_present_position)) 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
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
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
def PresentPos_spread(self): #Print current pos with goalpos global DXL_MOVING_STATUS_THRESHOLD #Read present position self.dxl_present_position = dynamixel.read2ByteTxRx( self.port_num, self.pro_ver, self.ID, self.addr_present) self.dxl_comm_result = dynamixel.getLastTxRxResult( self.port_num, self.pro_ver) self.dxl_error = dynamixel.getLastRxPacketError( self.port_num, self.pro_ver) if self.dxl_comm_result != self.COMM_SUCCESS: return (dynamixel.getTxRxResult(self.pro_ver, self.dxl_comm_result)) elif self.dxl_error != 0: return (dynamixel.getRxPacketError(self.pro_ver, self.dxl_error)) print("[ID:%03d] GoalPos:%03d PresPos:%03d" % (self.ID, self.goalpos, self.dxl_present_position)) if not (abs(self.goalpos - self.dxl_present_position) > DXL_MOVING_STATUS_THRESHOLD_SPREAD): return True return False
# Add Dynamixel#2 goal position value to the Syncwrite parameter storage dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncWriteAddParam(group_num, DXL2_ID, dxl_goal_position[index], LEN_MX_GOAL_POSITION)).value if dxl_addparam_result != 1: print("[ID:%03d] groupSyncWrite addparam failed" % (DXL2_ID)) # Syncwrite goal position dynamixel.groupSyncWriteTxPacket(group_num) if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) != COMM_SUCCESS: dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) # Clear syncwrite parameter storage dynamixel.groupSyncWriteClearParam(group_num) while 1: # Read Dynamixel#1 present position dxl1_present_position = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_MX_PRESENT_POSITION) 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)) # Read Dynamixel#2 present position dxl2_present_position = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_PRESENT_POSITION) 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)) print("[ID:%03d] GoalPos:%03d PresPos:%03d\t[ID:%03d] GoalPos:%03d PresPos:%03d" % (DXL1_ID, dxl_goal_position[index], dxl1_present_position, DXL2_ID, dxl_goal_position[index], dxl2_present_position)) if not ((abs(dxl_goal_position[index] - dxl1_present_position) > DXL_MOVING_STATUS_THRESHOLD) or (abs(dxl_goal_position[index] - dxl2_present_position) > DXL_MOVING_STATUS_THRESHOLD)):
elif dxl_error != 0: print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) while 1: # Read present position dxl_present_position = dynamixel.read4ByteTxRx( port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_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: print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) elif dxl_error != 0: print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) dxl_time = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_REALTIME_TICK) 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)) currentTimeuS = datetime.datetime.now().microsecond print( "[ID:%03d] GoalPos:%03d PresPos:%03d Device time %d Local time: %d" % (DXL_ID, dxl_goal_position[index], dxl_present_position, dxl_time, currentTimeuS)) if not (abs(dxl_goal_position[index] - dxl_present_position) > DXL_MOVING_STATUS_THRESHOLD):
elif dxl_error != 0: print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) # Write Dynamixel#2 goal position dynamixel.write2ByteTxRx(port_num2, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_GOAL_POSITION, dxl_goal_position[index]) dxl_comm_result = dynamixel.getLastTxRxResult(port_num2, PROTOCOL_VERSION) dxl_error = dynamixel.getLastRxPacketError(port_num2, 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)) while 1: # Read present position dxl1_present_position = dynamixel.read2ByteTxRx( port_num1, PROTOCOL_VERSION, DXL1_ID, ADDR_MX_PRESENT_POSITION) dxl_comm_result = dynamixel.getLastTxRxResult(port_num1, PROTOCOL_VERSION) dxl_error = dynamixel.getLastRxPacketError(port_num1, 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)) # Read present position dxl2_present_position = dynamixel.read2ByteTxRx( port_num2, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_PRESENT_POSITION) dxl_comm_result = dynamixel.getLastTxRxResult(port_num2, PROTOCOL_VERSION) dxl_error = dynamixel.getLastRxPacketError(port_num2, PROTOCOL_VERSION) if dxl_comm_result != COMM_SUCCESS:
def motor_operate(self, theta1_vp, theta2_vp, e, tx, ty, div_pt_x_vp, div_pt_y_vp): # ta1_vp and theta2_vp from list to float type theta1_vp = theta1_vp.tolist() theta2_vp = theta2_vp.tolist() for i in range(len(theta1_vp)): theta1_vp[i] = float(theta1_vp[i][0]) theta2_vp[i] = float(theta2_vp[i][0]) # Read a keypress and return the resulting character as a byte string. if os.name == 'nt': import msvcrt def getch(): return msvcrt.getch().decode() else: import sys, tty, termios fd = sys.stdin.fileno() old_settings = termios.tcgetattr(fd) def getch(): try: tty.setraw(sys.stdin.fileno()) ch = sys.stdin.read(1) finally: termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) return ch # Control table address ADDR_MX_TORQUE_ENABLE = 24 # 1 byte RW # Control table address is different in Dynamixel model ADDR_MX_GOAL_POSITION = 30 # 2 bytes RW ADDR_MX_PRESENT_POSITION = 36 # 2 bytes R ADDR_MX_MOVING_SPEED = 32 # 2 bytes RW ADDR_MX_PRESENT_SPEED = 38 # 2 bytes R ADDR_MX_PRESENT_LOAD = 40 # 2 bytes R ADDR_MX_PRESENT_VOLTAGE = 42 # 1 byte R ADDR_MX_GOAL_ACCELERATION = 73 # 1 byte RW ADDR_MX_MAX_TORQUE = 14 # 2 bytes RW # PID Controller Address ADDR_MX_DERIVATIVE_GAIN = 26 # 1 byte RW ADDR_MX_INTEGRAL_GAIN = 27 # 1 byte RW ADDR_MX_PROPORTIONAL_GAIN = 28 # 1 byte RW # Protocol version PROTOCOL_VERSION = 1 # See which protocol version is used in the Dynamixel # Default setting DXL_ID = [1, 2, 3, 4] BAUDRATE = 1000000 DEVICENAME = "COM7".encode('utf-8') # Check which port is being used on your controller # ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*" # Resolution is 0.088 degree / Position 0~4095 / TORQUE_ENABLE = 1 # Value for enabling the torque TORQUE_DISABLE = 0 # Value for disabling the torque DXL_MOVING_STATUS_THRESHOLD = 5 # Dynamixel moving status threshold ESC_ASCII_VALUE = 0x1b # for ESC key to escape out from the operation COMM_SUCCESS = 0 # Communication Success result value COMM_TX_FAIL = -1001 # Communication Tx Failed # Initialize PortHandler Structs # Set the port path # Get methods and members of PortHandlerLinux or PortHandlerWindows port_num = dynamixel.portHandler(DEVICENAME) # Initialize PacketHandler Structs dynamixel.packetHandler() index = 0 dxl_comm_result = COMM_TX_FAIL # Communication result dxl_error = 0 # Dynamixel error dxl_present_position = 0 # Present position theta1 = [] for i in range(len(theta1_vp)): theta1.append(int((180 * theta1_vp[i]) / (0.088 * np.pi)) + theta1_offset - 2048 + dynamixel_offset_90) print('=====================theta1 =======================') print(theta1) theta2 = [] for i in range(len(theta2_vp)): theta2.append(int((180 * theta2_vp[i]) / (0.088 * np.pi)) + theta2_offset - 2048 + 2048) print('=====================theta2=======================') print(theta2) theta3 = [theta3_offset] * len(theta2_vp) #theta4 = [2048] # Open port if dynamixel.openPort(port_num): print("Succeeded to open the port!") else: print("Failed to open the port!") print("Press any key to terminate...") getch() quit() # Set port baudrate 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() #set position P gain, I gain, D gain - all 1 byte - P only 254 for unit communication # Motor 1 dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_DERIVATIVE_GAIN, 254) dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_INTEGRAL_GAIN, 30) dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_PROPORTIONAL_GAIN, 80) # Motor 2 dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[1], ADDR_MX_DERIVATIVE_GAIN, 254) dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[1], ADDR_MX_INTEGRAL_GAIN, 50) dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[1], ADDR_MX_PROPORTIONAL_GAIN, 120) # Motor 3 dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[2], ADDR_MX_DERIVATIVE_GAIN, 254) dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[2], ADDR_MX_INTEGRAL_GAIN, 50) dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[2], ADDR_MX_PROPORTIONAL_GAIN, 120) # Motor 4 dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_DERIVATIVE_GAIN, 254) dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_INTEGRAL_GAIN, 50) dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_PROPORTIONAL_GAIN, 120) for id in DXL_ID: # Enable Dynamixel Torque - 1byte dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, id, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE) dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, id, ADDR_MX_MAX_TORQUE, 1024) dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) # Initialize Goal Position 2048 dxl_goal_position_init = 2048 dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE) dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_MAX_TORQUE, 1024) # Estimate time start_time = time.time() dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_GOAL_POSITION, 2200) dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[1], ADDR_MX_GOAL_POSITION, 2200) dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[2], ADDR_MX_GOAL_POSITION, 2048) dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_GOAL_POSITION, 2500) time.sleep(2) dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_GOAL_POSITION, theta1[0]) dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[1], ADDR_MX_GOAL_POSITION, theta2[0]) dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[2], ADDR_MX_GOAL_POSITION, theta3[0]) time.sleep(5) dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_GOAL_POSITION, 2038) time.sleep(4) while not (len(theta1) - index) == 0: # Write goal position dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_GOAL_POSITION, theta1[index]) dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[1], ADDR_MX_GOAL_POSITION, theta2[index]) dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[2], ADDR_MX_GOAL_POSITION, theta3[index]) dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_GOAL_POSITION, 2038) 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)) # write moving speed for id in DXL_ID: dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, id, ADDR_MX_MOVING_SPEED, 50) dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_MOVING_SPEED, 20) while 1: # Read present position dxl_present_position_theta1 = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_PRESENT_POSITION) dxl_present_position_theta2 = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[1], ADDR_MX_PRESENT_POSITION) dxl_present_position_theta3 = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[2], ADDR_MX_PRESENT_POSITION) dxl_present_position_theta4 = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_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: print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) elif dxl_error != 0: print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) # read voltage dxl_present_voltage = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_PRESENT_VOLTAGE) # read load dxl_present_load = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_PRESENT_LOAD) # read present speed dxl_present_vel = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_PRESENT_SPEED) #print("[ID:%03d] GoalPos:%03d PresPos:%03d PresVolt:%03d PresLoad:%03d PresVel:%03d" % ( #DXL_ID[0], theta2[index], dxl_present_position_theta2, #dxl_present_voltage, dxl_present_load, dxl_present_vel)) #print('e', e.is_set()) if e.is_set() == 0: pass elif e.is_set() != 0: dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_GOAL_POSITION, 2500) time.sleep(5) dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_GOAL_POSITION, 2200) dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[1], ADDR_MX_GOAL_POSITION, 2200) dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[2], ADDR_MX_GOAL_POSITION, 2048) #contour cam_paper = cv2.VideoCapture(0) ret_paper, frame_paper = cam_paper.read() frame_paper = cv2.flip(frame_paper, -1) cv2.imshow('frame', frame_paper) cv2.waitKey(1) cont_paper = contour_paper.ContourPaper() tx, ty = cont_paper.cont_paper(frame_paper) print('translated tx and ty', tx, ty) #paper move #new space for current theta1 and theta2 to return IKINE new_theta1 = np.zeros([len(theta1) - index, 1]) new_theta2 = np.zeros([len(theta2) - index, 1]) #new space of current theta1 and theta2 in degree theta1_deg = np.zeros([]) theta2_deg = np.zeros([]) theta1_deg = np.delete(theta1_deg, 0) theta2_deg = np.delete(theta2_deg, 0) #current theta1 and theta2, remove past theta value current_theta1 = theta1[index: ] current_theta2 = theta2[index: ] div_pt_x_vp = div_pt_x_vp[index: ] div_pt_y_vp = div_pt_y_vp[index: ] #print('x position, ', div_pt_x_vp) #print('imhere1') for i in range(len(current_theta1)): theta1_deg = np.append(theta1_deg, ((current_theta1[i] - theta1_offset + 2048 - dynamixel_offset_90) * np.pi * 0.088 / 180)) theta2_deg = np.append(theta2_deg, ((current_theta2[i] - theta2_offset) * np.pi * 0.088 / 180)) IK = ikine.InverseKinematics() for i in range(len(current_theta2)): #print('theta1_deg', theta1_deg[i]) new_theta1[i], new_theta2[i] = IK.ikine(np.int(div_pt_x_vp[i]), np.int(div_pt_y_vp[i]), tx, ty) #print('new_theta1, ', new_theta1) new_theta1 = new_theta1.tolist() new_theta2 = new_theta2.tolist() for i in range(len(new_theta1)): new_theta1[i] = float(new_theta1[i][0]) new_theta2[i] = float(new_theta2[i][0]) theta1 = [] for i in range(len(new_theta1)): theta1.append(int((180 * new_theta1[i]) / (0.088 * np.pi)) + theta1_offset - 2048 + dynamixel_offset_90) print('=====================theta1 =======================') print(theta1) theta2 = [] for i in range(len(new_theta2)): theta2.append(int((180 * new_theta2[i]) / (0.088 * np.pi)) + theta2_offset - 2048 + 2048) print('=====================theta2=======================') print(theta2) theta3 = [theta3_offset] * len(new_theta2) index = 0 time.sleep(5) break else: pass if not ((abs(theta1[index] - dxl_present_position_theta1) > DXL_MOVING_STATUS_THRESHOLD) | ((abs(theta2[index] - dxl_present_position_theta2)) > DXL_MOVING_STATUS_THRESHOLD) | ((abs(theta3[index] - dxl_present_position_theta3)) > DXL_MOVING_STATUS_THRESHOLD)) : break # check event #ABOUT TX and TY -> IKINE -> New theta1 and theta2 init_tx = tx init_ty = ty if e.is_set() != 0: print('error') dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_GOAL_POSITION, theta1[0]) dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[1], ADDR_MX_GOAL_POSITION, theta2[0]) dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[2], ADDR_MX_GOAL_POSITION, theta3[0]) time.sleep(5) dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_GOAL_POSITION, 2038) time.sleep(3) e.clear() else: pass index += 1 dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_GOAL_POSITION, 2500) time.sleep(5) # Disable Dynamixel Torque for id in DXL_ID: dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, id, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE) 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)) # Close port dynamixel.closePort(port_num) return tx, ty
if getch() == chr(ESC_ASCII_VALUE): break # Write goal position dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_GOAL_POSITION, dxl_goal_position[index]) 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)) while 1: # Read present position dxl_present_position = dynamixel.read2ByteTxRx( port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_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: print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) elif dxl_error != 0: print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) print("[ID:%03d] GoalPos:%03d PresPos:%03d" % (DXL_ID, dxl_goal_position[index], dxl_present_position)) if not (abs(dxl_goal_position[index] - dxl_present_position) > DXL_MOVING_STATUS_THRESHOLD): break