class eBot: # Initialise connection to pioneer on specified port def __init__(self): try: from soar.serial import Serial except ImportError: print "You are missing some serial support libraries. Probably you are on windows and you need to get pywin32. Check out http://sourceforge.net/projects/pywin32/ for details." raise CancelGUIAction self.sonarValues = [0, 0, 0, 0, 0, 0] self.all_Values = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0] self.port = None self.serialReady = False self.ldrvalue = [0, 0] self.p_value = [0, 0] self.acc_values = [0, 0, 0, 0, 0, 0] self.pos_values = [0, 0, 0] self.EKF = Locator_EKF([0., 0.], 0.) self.offset_counter = 0 self.thread_flag = 0 self.heading = 0. self.offset_counter_iteration = 200 self.Ultrasonic_rear_right = 0 self.Ultrasonic_right = 0 self.Ultrasonic_front = 0 self.Ultrasonic_left = 0 self.Ultrasonic_rear_left = 0 self.Ultrasonic_back = 0 self.portName = -1 self.sonarsChanged = [0, 0, 0, 0, 0, 0, 0, 0] #Change to 6 after self.from_update = 0 self.connectionState = STATE_NO_SYNC self.port = None self.lastxpos, self.lastypos = 0, 0 self.storedsonars = SharedVar([0, 0, 0, 0, 0, 0, 0, 0]) #Change to 6 after self.trans, self.rot = SharedVar(0), SharedVar(0) self.odpose = SharedVar((0, 0, 0)) self.stalled = SharedVar(False) self.analogInputs = SharedVar([0, 0, 0, 0]) self.analogOut = SharedVar(0) self.asynchronous = True self.zero_recv_cnt = 0 # Added -> Not in pioneer self.setters = { "motorOutput": self.motorOutput, "discreteMotorOutput": self.discreteMotorOutput, "say": self.cmdSay, "analogOutput": self.analogOutput, "setMaxEffectiveSonarDistance": self.setMaxEffectiveSonarDistance, "enableAccelerationCap": self.enableAccelerationCap, "setMaxVelocities": self.setMaxVelocities } self.getters = { "pose": self.odpose.get, "sonarDistances": self.storedsonars.get, "stalled": self.stalled.get, "analogInputs": self.analogInputs.get } debug("eBot variables set up", 2) self.serialReady = False self.prev_sonar = [0, 0, 0, 0, 0, 0] try: self.connect(Serial) self.open() debug("Serial Connection started", 2) print "eBot connection successful" except: sys.stderr.write("Couldn't connect to eBot.\n") sys.stderr.write( "- Check if eBot is on, paired and connected. If not, power up and try again. \n" ) #sys.stderr.write("- Check to see if COM port below is eBot. If not, remove device and try again. \n") debug("Could not open the serial port", 0) print 5 raise CancelGUIAction("Error opening serial port") app.soar.addFlowTriplet( (self.startmoving, self.update, self.stopmoving)) self.currentthread = None self.acceptMotorCmds = False self.startSerialThread() sleep(7) def destroy(self): self.stopSerialThread() sleep(0.2) if (self.serialReady): #self.cmdEnable(0) self.stopmoving() self.sendOutputs() self.sendPacket([CMD.CLOSE]) # self.flushSerialPort() try: self.port.close() except: pass app.soar.removeFlowTriplet( (self.startmoving, self.update, self.stopmoving)) # yes, this is repeated code, and should be neater. you fix it. def initGlobals(self, dummy): self.stopSerialThread() sleep(0.2) #if self.serialReady: #self.cmdEnable(0) #self.stopmoving() #self.sendPacket([ArcosCmd.CLOSE]) self.connectionState = STATE_NO_SYNC self.sonarsChanged = [0, 0, 0, 0, 0, 0, 0, 0] from soar.serial import Serial self.connect(Serial) self.open() #self.cmdEnable(1) self.startSerialThread() self.odpose.set((0.0, 0.0, 0.0)) def getOpenPorts(self): """ This Function Returns a list of tuples with the port number and its description. Used for Windows only """ path = 'HARDWARE\\DEVICEMAP\\SERIALCOMM' key = winreg.OpenKey(winreg.HKEY_LOCAL_MACHINE, path) ports = [] #maximum 256 entries, will break anyways for i in range(256): try: val = winreg.EnumValue(key, i) port = (str(val[1]), str(val[0])) ports.append(port) except Exception: winreg.CloseKey(key) break #return ports #ports = getOpenPorts() devicePorts = [] for port in ports: #Just because it is formatted that way... if 'BthModem' in port[1][8:] or 'VCP' in port[1][ 8:] or 'ProlificSerial' in port[1][8:]: devicePorts.append((int(port[0][3:]) - 1)) return devicePorts def connect(self, Serial): baudRate = 115200 ports = [] if os.name == "posix": if sys.platform == "linux2": #usbSerial = glob.glob('/dev/ttyUSB*') ports = glob.glob('/dev/rfcomm*') elif sys.platform == "darwin": ports = glob.glob('/dev/tty.eBot*') #usbSerial = glob.glob('/dev/tty.usbserial*') else: print "Unknown posix OS." sys.exit() elif os.name == "nt": ports = self.getOpenPorts() #ports = ['COM' + str(i + 1) for i in range(256)] #EBOT_PORTS = getEBotPorts() connect = 0 ebot_ports = [] ebot_names = [] for port in ports: try: s = Serial(port, baudRate, timeout=1, writeTimeout=1) s._timeout = 1.0 s._writeTimeout = 1.0 #try: # s.open() #except: # continue line = "aaaa" while line[:2] != "eB": if (s.inWaiting() > 0): line = s.readline() s.write("<<1?") sleep(0.5) line = s.readline() if (line[:2] == "eB"): ebot_ports.append(port) ebot_names.append(line) connect = 1 self.port = s self.portName = port self.port._timeout = 1.0 self.port._writeTimeout = 1.0 self.port.flushInput() self.port.flushOutput() if (line[:2] == "eB"): break #s.close() # self. except: try: if s.isOpen(): s.close() except: pass if (connect == 0): try: self.port.close() except: pass #sys.stderr.write("Could not open serial port. Is robot turned on and connected?\n") raise Exception("No Robot Found") # Open connection to robot def open(self): numAttempts = 3 timeout = 5.0 startt = time() while (self.connectionState != STATE_READY): if (self.connectionState == STATE_NO_SYNC): self.sendPacket([CMD_SYNC_SEND.CHECK1]) elif (self.connectionState == STATE_SYNC): self.sendPacket([CMD_SYNC_SEND.CHECK2]) self.connectionState = STATE_READY break #elif (self.connectionState == STATE_READY): # pass try: pkt = self.recvPacket() except: if (self.connectionState == STATE_NO_SYNC and numAttempts >= 0): numAttempts -= 1 else: self.port.close() sys.stderr.write( "Could not open serial port. Is robot turned on and connected?\n" ) raise Exception("No Robot Found") continue if (pkt[3] == CMD_SYNC_RECV.CHECK1): self.connectionState = STATE_SYNC # elif (pkt[3]==CMD_SYNC_RECV.CHECK2): # self.connectionState = STATE_READY if (time() - startt) > timeout: self.port.close() sys.stderr.write("Robot needs to be reset.\n") raise Exception("Bad Robot State") sleep(.01) try: self.port.write('A') except: sys.stderr.write("Could not write to serial port.\n") self.serialReady = False app.soar.closeEBot() sys.stderr.write( "Robot turned off or no longer connected.\nDead reckoning is no longer valid.\n" ) #changed = [0, 0, 0, 0, 0, 0] #while 0 in changed: # #self.cmdPulse() # self.sipRecv() # self.storedsonars.get() # changed = [changed[i] or self.sonarsChanged[i] for i in range(len(changed))] self.serialReady = True # turn on io packets #self.sendPacket([ArcosCmd.IOREQUEST,ArgType.ARGINT,2,0]) def getPose(self): return self.odpose.get() def initGlobals(self): self.odpose.set(0.0, 0.0, 0.0) def flushSerial(self): self.port.flushInput() self.port.flushOutput() def setMaxEffectiveSonarDistance(self, d): pass def enableAccelerationCap(self, enable): if not enable: sys.stderr.write("Can't disable accleration cap on real robot.\n") def setMaxVelocities(self, maxTransVel, maxRotVel): global MAX_TRANS, MAX_ROT MAX_TRANS = maxTransVel MAX_ROT = maxRotVel if maxTransVel > 0.75: sys.stderr.write( "Trying to set maximum translational velocity too high for real robot\n" ) if maxRotVel > 1.75: sys.stderr.write( "Trying to set maximum rotational velocity too high for real robot\n" ) def enableTeleportation(self, prob, pose): sys.stderr.write( "Enabling teleportation on real robot has no effect.\n") # Move by translating with velocity v (m/sec), # and rotating with velocity r (rad/sec) def motorOutput(self, v, w): # traceback.print_stack() if self.acceptMotorCmds: self.trans.set(clip(v, -MAX_TRANS, MAX_TRANS)) self.rot.set(clip(w, -MAX_ROT, MAX_ROT)) if (not self.asynchronous): self.update() def analogOutput(self, v): self.analogOut.set(clip(v, 0.0, 10.0)) # Move by translating with velocity v (m/sec), # and rotating with velocity r (deg/sec) # for dt seconds def discreteMotorOutput(self, v, w, dt): if self.acceptMotorCmds: self.motorOutput(v, w) sleep(dt) self.motorOutput(0, 0) def sendOutputs(self): data = [CMD.RECV] v = int(self.trans.get() * 1000.0) if (v >= 0): #data = [abs(v) & 0xFF, v >> 8, 0x01] data.append(abs(v) / 256) data.append(abs(v) % 256) data.append(0x01) else: #data = [abs(v) & 0xFF, v >> 8, 0x10] data.append(abs(v) / 256) data.append(abs(v) % 256) data.append(0x10) rv = int(self.rot.get() * 220 / pi) # 180*1.5 if (rv >= 0): data.append(abs(rv) / 256) data.append(abs(rv) % 256) data.append(0x01) else: data.append(abs(rv) / 256) data.append(abs(rv) % 256) data.append(0x10) self.sipSend(data) def sendMotorCmd(self): self.cmdVel(int(self.trans.get() * 1000.0)) self.cmdRvel(int(self.rot.get() * 180 / pi)) def sendDigitalOutCmd(self): # high order byte selects output bit for change # low order byte sets/resets bit data = [ ArcosCmd.DIGOUT, ArgType.ARGINT, # convert 0-10 voltage to 0-255 value (int(math.floor((self.analogOut.get() * 25.5))) & 0xFF), 0xFF ] self.sipSend(data) # update sonars, odometry and stalled def read(self): self.incoming = None if self.offset_counter > 1: self.prev_time_stamp = self.time_stamp if self.port.inWaiting() != 0: self.incoming = self.port.readline().rstrip('\n') self.offset_counter += 1 try: self.time_stamp , self.Ax , self.Ay , self.Az , self.Gx , self.Gy , self.Gz , \ self.Ultrasonic_rear_right, self.Ultrasonic_right, self.Ultrasonic_front , \ self.Ultrasonic_left , self.Ultrasonic_rear_left , self.Ultrasonic_back , \ self.encoder_right , self.encoder_left , self.LDR_top , self.LDR_front , \ self.tempreture_sensor , self.voltage , self.current = self.incoming.split(";") self.time_stamp = float(self.time_stamp) self.Ax = float(self.Ax) self.Ay = float(self.Ay) self.Az = float(self.Az) self.Gx = float(self.Gx) self.Gy = float(self.Gy) self.Gz = float(self.Gz) self.Ultrasonic_rear_right = float(self.Ultrasonic_rear_right) self.Ultrasonic_right = float(self.Ultrasonic_right) self.Ultrasonic_front = float(self.Ultrasonic_front) self.Ultrasonic_left = float(self.Ultrasonic_left) self.Ultrasonic_rear_left = float(self.Ultrasonic_rear_left) self.Ultrasonic_back = float(self.Ultrasonic_back) self.encoder_right = float(self.encoder_right) self.encoder_left = float(self.encoder_left) self.LDR_top = float(self.LDR_top) self.LDR_front = float(self.LDR_front) self.tempreture_sensor = float(self.tempreture_sensor) self.voltage = float(self.voltage) self.current = float(self.current) except: print "eBot.read(): bad formatted data received" print self.incoming if self.offset_counter == 2: self.Ax_offset = self.Ax self.Ay_offset = self.Ay self.Az_offset = self.Az self.Gx_offset = self.Gx self.Gy_offset = self.Gy self.Gz_offset = self.Gz if self.offset_counter > 2 and self.offset_counter < self.offset_counter_iteration: # we just average the first 50 # readings of the accelerometer and Gyro sensor and find the offset value by averaging them self.Ax_offset = (self.Ax + self.Ax_offset) self.Ay_offset = (self.Ay + self.Ay_offset) self.Az_offset = (self.Az + self.Az_offset) self.Gx_offset = (self.Gx + self.Gx_offset) self.Gy_offset = (self.Gy + self.Gy_offset) self.Gz_offset = (self.Gz + self.Gz_offset) if self.offset_counter == self.offset_counter_iteration: # we just average the first 50 # readings of the accelerometer and Gyro sensor and find the offset value by averaging them self.Ax_offset = self.Ax_offset / self.offset_counter_iteration self.Ay_offset = self.Ay_offset / self.offset_counter_iteration self.Az_offset = self.Az_offset / self.offset_counter_iteration self.Gx_offset = self.Gx_offset / self.offset_counter_iteration self.Gy_offset = self.Gy_offset / self.offset_counter_iteration self.Gz_offset = self.Gz_offset / self.offset_counter_iteration if self.offset_counter > self.offset_counter_iteration: sampling_time = (self.time_stamp - self.prev_time_stamp) / 1000. if sampling_time > 0: if abs(self.Gz - self.Gz_offset) > 50: # to remove the noise self.heading = self.heading + sampling_time * ( self.Gz - self.Gz_offset ) # the integration to get the heading self.heading_scaled = self.heading / 130.5 #128.7 self.heading_scaled = self.heading_scaled self.pos_values[0],self.pos_values[1],self.pos_values[2] = \ self.EKF.update_state([self.heading_scaled*pi/180.,self.encoder_right/1000.,self.encoder_left/1000.],sampling_time) return self.incoming def update(self, dummyparameter=0): if (self.serialReady): #self.sendPacket([CMD.RECV]) #self.sendMotorCmd() #self.sendDigitalOutCmd() # temp = self.port.inWaiting() self.sendOutputs() #sleep(.005) #self.sendPacket([CMD.SEND]) self.read() sleep(.005) self.parseData() self.from_update = 1 #if (self.sipRecv() > 0): # self.zero_recv_cnt = 0 #else: # self.zero_recv_cnt += 1 # if (self.zero_recv_cnt > 400): # self.serialReady = False # app.soar.closeEBot() # sys.stderr.write("Robot turned off or no longer connected.\nDead reckoning is no longer valid.\n") # Calculate checksum on P2OS packet (see Pioneer manual) def calcChecksum(self, data): c = 0 i = 3 n = data[2] - ord('0') - 2 while (n > 1): c += (data[i] << 8) | (data[i + 1]) c = c & 0xFFFF n -= 2 i += 2 if (n > 0): c = c ^ data[i] return c def startmoving(self): self.acceptMotorCmds = True # if (self.asynchronous): # self.stopmoving() # self.startSerialThread() def stopmoving(self): self.motorOutput(0.0, 0.0) self.acceptMotorCmds = False if (self.asynchronous): pass # self.stopSerialThread() else: #self.sendMotorCmd() self.sendOutputs() def startSerialThread(self): self.flushSerial() self.currentthread = Stepper(self.update, 50) self.currentthread.start() def stopSerialThread(self): try: self.currentthread.stop() except AttributeError: pass # Send a packet to robot def sendPacket(self, data): #pkt = [0x3C,0x3C,len(data)+2] pkt = [0x3C, 0x3C, len(data)] for d in data: pkt.append(d) #pkt.append(0) #pkt.append(0) #chk = self.calcChecksum(pkt) #pkt[len(pkt)-2] = (chk>>8) #pkt[len(pkt)-1] = (chk&0xFF) s = reduce(lambda x, y: x + chr(y), pkt, "") try: self.port.write(s) except: sys.stderr.write("Could not write to serial port.\n") self.serialReady = False app.soar.closeEBot() sys.stderr.write( "Robot turned off or no longer connected.\nDead reckoning is no longer valid.\n" ) sleep(0.008) # Block until packet recieved def recvPacket(self): timeout = 1.0 data = [0, 0, 0] while (1): cnt = 0 tstart = time() while (time() - tstart) < timeout: if (self.port.inWaiting() > 0): data[2] = ord(self.port.read()) break if (time() - tstart) > timeout: raise Exception("Read timeout") if (data[0] == 0x3E and data[1] == 0x3E): break data[0] = data[1] data[1] = data[2] for d in range(data[2] - ord('0')): data.append(ord(self.port.read())) #if self.from_update: #crc = self.calcChecksum(data) #if data[len(data)-1]!=(crc&0xFF) or data[len(data)-2]!=(crc>>8): # self.port.flushInput() # raise Exception("Checksum failure") #if self.port.inWaiting() > 0: # self.port.flushInput() #self.from_update=0 return data # Send a packet def sipSend(self, data): if self.serialReady: self.sendPacket(data) # Receive all waiting packets. # returns the number of packets read def sipRecv(self): iters = 0 if self.serialReady: while (self.port.inWaiting() > 0): try: recv = self.recvPacket() except: print 'no recv packet' #continue break iters += 1 self.parseData(recv) # 0x3s means sip packet #if (recv[3]&0xF0)==0x30: # self.parseSip(recv) # 0xF0 means io packet #elif recv[3]==0xF0: # self.parseIO(recv) return iters def parseData(self): # parse all data stall = 0 bump = 0 sonars = [-1, -1, -1, -1, -1, -1] #Right Sonar sonars[4] = float(self.Ultrasonic_rear_right) #if -0.5<self.prev_sonar[4]- sonars[4]<0.5: # sonars[4] = self.prev_sonar4 #else: # self.prev_sonar4=sonars[4] #print 'Sonar 1: ', sonars[0] #Right Front Sonar sonars[3] = float(self.Ultrasonic_right) #print 'Sonar 2: ', sonars[1] #Front Sonar sonars[2] = float(self.Ultrasonic_front) #print 'Sonar 3: ', sonars[2] #Left Front Sonar sonars[1] = float(self.Ultrasonic_left) #print 'Sonar 4: ', sonars[3 #Left Sonar sonars[0] = float(self.Ultrasonic_rear_left) #print 'Sonar 5: ', sonars[4] #Back Sonar sonars[5] = float(self.Ultrasonic_back) #print 'Sonar 6: ', sonars[5] self.pos_values[0], self.pos_values[1] = self.EKF.get_position() self.pos_values[2] = self.EKF.get_heading() self.pos_values[2] = self.pos_values[2] % (2 * pi) if self.pos_values[2] > 2 * pi: self.pos_values[2] -= 2 * pi elif self.pos_values[2] < 0: self.pos_values[2] += 2 * pi self.odpose.set( (self.pos_values[0], self.pos_values[1], self.pos_values[2])) self.stalled.set((stall != 0x00) or (bump != 0x00)) storedsonars = self.storedsonars.get() for i in range(len(sonars)): if sonars[i] != -1: storedsonars[i] = sonars[i] * METER_SCALE self.sonarsChanged[i] = 1 else: self.sonarsChanged[i] = 0 self.storedsonars.set(storedsonars) # Send a packet and receive a SIP response from the robot def sipSendReceive(self, data): self.sipSend(data) self.sipRecv() # def cmdPulse(self): # self.sipSendReceive([ArcosCmd.PULSE]) # def cmdPulseSend(self): # self.sipSend([ArcosCmd.PULSE]) def cmdEnable(self, v): #self.sendPacket([ArcosCmd.ENABLE, ArgType.ARGINT, v, 0]) pass def cmdVel(self, v): absv = int(abs(v)) if (v >= 0): data = [ArcosCmd.VEL, ArgType.ARGINT, absv & 0xFF, absv >> 8] else: data = [ArcosCmd.VEL, ArgType.ARGNINT, absv & 0xFF, absv >> 8] # self.sipSendReceive(data) self.sipSend(data) # def cmdVel2(self,l,r): # self.sipSendReceive([ArcosCmd.VEL2,ArgType.ARGINT,int(r),int(l)]) def cmdRvel(self, rv): absrv = abs(rv) if (rv >= 0): data = [ArcosCmd.RVEL, ArgType.ARGINT, absrv & 0xFF, absrv >> 8] else: data = [ArcosCmd.RVEL, ArgType.ARGNINT, absrv & 0xFF, absrv >> 8] # self.sipSendReceive(data) self.sipSend(data) def cmdSay(self, note, duration): data = [ArcosCmd.SAY, ArgType.ARGSTR] for d in ("%s,%s" % (note, duration)): data.append(ord(d)) data.append(0)
class eBot: # Initialise connection to pioneer on specified port def __init__(self): try: from soar.serial import Serial except ImportError: print "You are missing some serial support libraries. Probably you are on windows and you need to get pywin32. Check out http://sourceforge.net/projects/pywin32/ for details." raise CancelGUIAction self.sonarValues = [0, 0, 0, 0, 0, 0] self.all_Values = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0] self.port = None self.serialReady = False self.ldrvalue = [0, 0] self.p_value = [0, 0] self.acc_values = [0, 0, 0, 0, 0, 0] self.pos_values = [0, 0, 0] self.EKF = Locator_EKF([0.0, 0.0], 0.0) self.offset_counter = 0 self.thread_flag = 0 self.heading = 0.0 self.offset_counter_iteration = 200 self.Ultrasonic_rear_right = 0 self.Ultrasonic_right = 0 self.Ultrasonic_front = 0 self.Ultrasonic_left = 0 self.Ultrasonic_rear_left = 0 self.Ultrasonic_back = 0 self.portName = -1 self.sonarsChanged = [0, 0, 0, 0, 0, 0, 0, 0] # Change to 6 after self.from_update = 0 self.connectionState = STATE_NO_SYNC self.port = None self.lastxpos, self.lastypos = 0, 0 self.storedsonars = SharedVar([0, 0, 0, 0, 0, 0, 0, 0]) # Change to 6 after self.trans, self.rot = SharedVar(0), SharedVar(0) self.odpose = SharedVar((0, 0, 0)) self.stalled = SharedVar(False) self.analogInputs = SharedVar([0, 0, 0, 0]) self.analogOut = SharedVar(0) self.asynchronous = True self.zero_recv_cnt = 0 # Added -> Not in pioneer self.setters = { "motorOutput": self.motorOutput, "discreteMotorOutput": self.discreteMotorOutput, "say": self.cmdSay, "analogOutput": self.analogOutput, "setMaxEffectiveSonarDistance": self.setMaxEffectiveSonarDistance, "enableAccelerationCap": self.enableAccelerationCap, "setMaxVelocities": self.setMaxVelocities, } self.getters = { "pose": self.odpose.get, "sonarDistances": self.storedsonars.get, "stalled": self.stalled.get, "analogInputs": self.analogInputs.get, } debug("eBot variables set up", 2) self.serialReady = False self.prev_sonar = [0, 0, 0, 0, 0, 0] try: self.connect(Serial) self.open() debug("Serial Connection started", 2) print "eBot connection successful" except: sys.stderr.write("Couldn't connect to eBot.\n") sys.stderr.write("- Check if eBot is on, paired and connected. If not, power up and try again. \n") # sys.stderr.write("- Check to see if COM port below is eBot. If not, remove device and try again. \n") debug("Could not open the serial port", 0) print 5 raise CancelGUIAction("Error opening serial port") app.soar.addFlowTriplet((self.startmoving, self.update, self.stopmoving)) self.currentthread = None self.acceptMotorCmds = False self.startSerialThread() sleep(7) def destroy(self): self.stopSerialThread() sleep(0.2) if self.serialReady: # self.cmdEnable(0) self.stopmoving() self.sendOutputs() self.sendPacket([CMD.CLOSE]) # self.flushSerialPort() try: self.port.close() except: pass app.soar.removeFlowTriplet((self.startmoving, self.update, self.stopmoving)) # yes, this is repeated code, and should be neater. you fix it. def initGlobals(self, dummy): self.stopSerialThread() sleep(0.2) # if self.serialReady: # self.cmdEnable(0) # self.stopmoving() # self.sendPacket([ArcosCmd.CLOSE]) self.connectionState = STATE_NO_SYNC self.sonarsChanged = [0, 0, 0, 0, 0, 0, 0, 0] from soar.serial import Serial self.connect(Serial) self.open() # self.cmdEnable(1) self.startSerialThread() self.odpose.set((0.0, 0.0, 0.0)) def getOpenPorts(self): """ This Function Returns a list of tuples with the port number and its description. Used for Windows only """ path = "HARDWARE\\DEVICEMAP\\SERIALCOMM" key = winreg.OpenKey(winreg.HKEY_LOCAL_MACHINE, path) ports = [] # maximum 256 entries, will break anyways for i in range(256): try: val = winreg.EnumValue(key, i) port = (str(val[1]), str(val[0])) ports.append(port) except Exception: winreg.CloseKey(key) break # return ports # ports = getOpenPorts() devicePorts = [] for port in ports: # Just because it is formatted that way... if "BthModem" in port[1][8:] or "VCP" in port[1][8:] or "ProlificSerial" in port[1][8:]: devicePorts.append((int(port[0][3:]) - 1)) return devicePorts def connect(self, Serial): baudRate = 115200 ports = [] if os.name == "posix": if sys.platform == "linux2": # usbSerial = glob.glob('/dev/ttyUSB*') print "Support for this OS is under development." elif sys.platform == "darwin": ports = glob.glob("/dev/tty.eBot*") # usbSerial = glob.glob('/dev/tty.usbserial*') else: print "Unknown posix OS." sys.exit() elif os.name == "nt": ports = self.getOpenPorts() # ports = ['COM' + str(i + 1) for i in range(256)] # EBOT_PORTS = getEBotPorts() connect = 0 ebot_ports = [] ebot_names = [] for port in ports: try: s = Serial(port, baudRate, timeout=1, writeTimeout=1) s._timeout = 1.0 s._writeTimeout = 1.0 # try: # s.open() # except: # continue line = "aaaa" while line[:2] != "eB": if s.inWaiting() > 0: line = s.readline() s.write("<<1?") sleep(0.5) line = s.readline() if line[:2] == "eB": ebot_ports.append(port) ebot_names.append(line) connect = 1 self.port = s self.portName = port self.port._timeout = 1.0 self.port._writeTimeout = 1.0 self.port.flushInput() self.port.flushOutput() if line[:2] == "eB": break # s.close() # self. except: try: if s.isOpen(): s.close() except: pass if connect == 0: try: self.port.close() except: pass # sys.stderr.write("Could not open serial port. Is robot turned on and connected?\n") raise Exception("No Robot Found") # Open connection to robot def open(self): numAttempts = 3 timeout = 5.0 startt = time() while self.connectionState != STATE_READY: if self.connectionState == STATE_NO_SYNC: self.sendPacket([CMD_SYNC_SEND.CHECK1]) elif self.connectionState == STATE_SYNC: self.sendPacket([CMD_SYNC_SEND.CHECK2]) self.connectionState = STATE_READY break # elif (self.connectionState == STATE_READY): # pass try: pkt = self.recvPacket() except: if self.connectionState == STATE_NO_SYNC and numAttempts >= 0: numAttempts -= 1 else: self.port.close() sys.stderr.write("Could not open serial port. Is robot turned on and connected?\n") raise Exception("No Robot Found") continue if pkt[3] == CMD_SYNC_RECV.CHECK1: self.connectionState = STATE_SYNC # elif (pkt[3]==CMD_SYNC_RECV.CHECK2): # self.connectionState = STATE_READY if (time() - startt) > timeout: self.port.close() sys.stderr.write("Robot needs to be reset.\n") raise Exception("Bad Robot State") sleep(0.01) try: self.port.write("A") except: sys.stderr.write("Could not write to serial port.\n") self.serialReady = False app.soar.closeEBot() sys.stderr.write("Robot turned off or no longer connected.\nDead reckoning is no longer valid.\n") # changed = [0, 0, 0, 0, 0, 0] # while 0 in changed: # #self.cmdPulse() # self.sipRecv() # self.storedsonars.get() # changed = [changed[i] or self.sonarsChanged[i] for i in range(len(changed))] self.serialReady = True # turn on io packets # self.sendPacket([ArcosCmd.IOREQUEST,ArgType.ARGINT,2,0]) def getPose(self): return self.odpose.get() def initGlobals(self): self.odpose.set(0.0, 0.0, 0.0) def flushSerial(self): self.port.flushInput() self.port.flushOutput() def setMaxEffectiveSonarDistance(self, d): pass def enableAccelerationCap(self, enable): if not enable: sys.stderr.write("Can't disable accleration cap on real robot.\n") def setMaxVelocities(self, maxTransVel, maxRotVel): global MAX_TRANS, MAX_ROT MAX_TRANS = maxTransVel MAX_ROT = maxRotVel if maxTransVel > 0.75: sys.stderr.write("Trying to set maximum translational velocity too high for real robot\n") if maxRotVel > 1.75: sys.stderr.write("Trying to set maximum rotational velocity too high for real robot\n") def enableTeleportation(self, prob, pose): sys.stderr.write("Enabling teleportation on real robot has no effect.\n") # Move by translating with velocity v (m/sec), # and rotating with velocity r (rad/sec) def motorOutput(self, v, w): # traceback.print_stack() if self.acceptMotorCmds: self.trans.set(clip(v, -MAX_TRANS, MAX_TRANS)) self.rot.set(clip(w, -MAX_ROT, MAX_ROT)) if not self.asynchronous: self.update() def analogOutput(self, v): self.analogOut.set(clip(v, 0.0, 10.0)) # Move by translating with velocity v (m/sec), # and rotating with velocity r (deg/sec) # for dt seconds def discreteMotorOutput(self, v, w, dt): if self.acceptMotorCmds: self.motorOutput(v, w) sleep(dt) self.motorOutput(0, 0) def sendOutputs(self): data = [CMD.RECV] v = int(self.trans.get() * 1000.0) if v >= 0: # data = [abs(v) & 0xFF, v >> 8, 0x01] data.append(abs(v) / 256) data.append(abs(v) % 256) data.append(0x01) else: # data = [abs(v) & 0xFF, v >> 8, 0x10] data.append(abs(v) / 256) data.append(abs(v) % 256) data.append(0x10) rv = int(self.rot.get() * 220 / pi) # 180*1.5 if rv >= 0: data.append(abs(rv) / 256) data.append(abs(rv) % 256) data.append(0x01) else: data.append(abs(rv) / 256) data.append(abs(rv) % 256) data.append(0x10) self.sipSend(data) def sendMotorCmd(self): self.cmdVel(int(self.trans.get() * 1000.0)) self.cmdRvel(int(self.rot.get() * 180 / pi)) def sendDigitalOutCmd(self): # high order byte selects output bit for change # low order byte sets/resets bit data = [ ArcosCmd.DIGOUT, ArgType.ARGINT, # convert 0-10 voltage to 0-255 value (int(math.floor((self.analogOut.get() * 25.5))) & 0xFF), 0xFF, ] self.sipSend(data) # update sonars, odometry and stalled def read(self): self.incoming = None if self.offset_counter > 1: self.prev_time_stamp = self.time_stamp if self.port.inWaiting() != 0: self.incoming = self.port.readline().rstrip("\n") self.offset_counter += 1 try: self.time_stamp, self.Ax, self.Ay, self.Az, self.Gx, self.Gy, self.Gz, self.Ultrasonic_rear_right, self.Ultrasonic_right, self.Ultrasonic_front, self.Ultrasonic_left, self.Ultrasonic_rear_left, self.Ultrasonic_back, self.encoder_right, self.encoder_left, self.LDR_top, self.LDR_front, self.tempreture_sensor, self.voltage, self.current = self.incoming.split( ";" ) self.time_stamp = float(self.time_stamp) self.Ax = float(self.Ax) self.Ay = float(self.Ay) self.Az = float(self.Az) self.Gx = float(self.Gx) self.Gy = float(self.Gy) self.Gz = float(self.Gz) self.Ultrasonic_rear_right = float(self.Ultrasonic_rear_right) self.Ultrasonic_right = float(self.Ultrasonic_right) self.Ultrasonic_front = float(self.Ultrasonic_front) self.Ultrasonic_left = float(self.Ultrasonic_left) self.Ultrasonic_rear_left = float(self.Ultrasonic_rear_left) self.Ultrasonic_back = float(self.Ultrasonic_back) self.encoder_right = float(self.encoder_right) self.encoder_left = float(self.encoder_left) self.LDR_top = float(self.LDR_top) self.LDR_front = float(self.LDR_front) self.tempreture_sensor = float(self.tempreture_sensor) self.voltage = float(self.voltage) self.current = float(self.current) except: print "eBot.read(): bad formatted data received" print self.incoming if self.offset_counter == 2: self.Ax_offset = self.Ax self.Ay_offset = self.Ay self.Az_offset = self.Az self.Gx_offset = self.Gx self.Gy_offset = self.Gy self.Gz_offset = self.Gz if ( self.offset_counter > 2 and self.offset_counter < self.offset_counter_iteration ): # we just average the first 50 # readings of the accelerometer and Gyro sensor and find the offset value by averaging them self.Ax_offset = self.Ax + self.Ax_offset self.Ay_offset = self.Ay + self.Ay_offset self.Az_offset = self.Az + self.Az_offset self.Gx_offset = self.Gx + self.Gx_offset self.Gy_offset = self.Gy + self.Gy_offset self.Gz_offset = self.Gz + self.Gz_offset if self.offset_counter == self.offset_counter_iteration: # we just average the first 50 # readings of the accelerometer and Gyro sensor and find the offset value by averaging them self.Ax_offset = self.Ax_offset / self.offset_counter_iteration self.Ay_offset = self.Ay_offset / self.offset_counter_iteration self.Az_offset = self.Az_offset / self.offset_counter_iteration self.Gx_offset = self.Gx_offset / self.offset_counter_iteration self.Gy_offset = self.Gy_offset / self.offset_counter_iteration self.Gz_offset = self.Gz_offset / self.offset_counter_iteration if self.offset_counter > self.offset_counter_iteration: sampling_time = (self.time_stamp - self.prev_time_stamp) / 1000.0 if sampling_time > 0: if abs(self.Gz - self.Gz_offset) > 50: # to remove the noise self.heading = self.heading + sampling_time * ( self.Gz - self.Gz_offset ) # the integration to get the heading self.heading_scaled = self.heading / 130.5 # 128.7 self.heading_scaled = self.heading_scaled self.pos_values[0], self.pos_values[1], self.pos_values[2] = self.EKF.update_state( [self.heading_scaled * pi / 180.0, self.encoder_right / 1000.0, self.encoder_left / 1000.0], sampling_time, ) return self.incoming def update(self, dummyparameter=0): if self.serialReady: # self.sendPacket([CMD.RECV]) # self.sendMotorCmd() # self.sendDigitalOutCmd() # temp = self.port.inWaiting() self.sendOutputs() # sleep(.005) # self.sendPacket([CMD.SEND]) self.read() sleep(0.005) self.parseData() self.from_update = 1 # if (self.sipRecv() > 0): # self.zero_recv_cnt = 0 # else: # self.zero_recv_cnt += 1 # if (self.zero_recv_cnt > 400): # self.serialReady = False # app.soar.closeEBot() # sys.stderr.write("Robot turned off or no longer connected.\nDead reckoning is no longer valid.\n") # Calculate checksum on P2OS packet (see Pioneer manual) def calcChecksum(self, data): c = 0 i = 3 n = data[2] - ord("0") - 2 while n > 1: c += (data[i] << 8) | (data[i + 1]) c = c & 0xFFFF n -= 2 i += 2 if n > 0: c = c ^ data[i] return c def startmoving(self): self.acceptMotorCmds = True # if (self.asynchronous): # self.stopmoving() # self.startSerialThread() def stopmoving(self): self.motorOutput(0.0, 0.0) self.acceptMotorCmds = False if self.asynchronous: pass # self.stopSerialThread() else: # self.sendMotorCmd() self.sendOutputs() def startSerialThread(self): self.flushSerial() self.currentthread = Stepper(self.update, 50) self.currentthread.start() def stopSerialThread(self): try: self.currentthread.stop() except AttributeError: pass # Send a packet to robot def sendPacket(self, data): # pkt = [0x3C,0x3C,len(data)+2] pkt = [0x3C, 0x3C, len(data)] for d in data: pkt.append(d) # pkt.append(0) # pkt.append(0) # chk = self.calcChecksum(pkt) # pkt[len(pkt)-2] = (chk>>8) # pkt[len(pkt)-1] = (chk&0xFF) s = reduce(lambda x, y: x + chr(y), pkt, "") try: self.port.write(s) except: sys.stderr.write("Could not write to serial port.\n") self.serialReady = False app.soar.closeEBot() sys.stderr.write("Robot turned off or no longer connected.\nDead reckoning is no longer valid.\n") sleep(0.008) # Block until packet recieved def recvPacket(self): timeout = 1.0 data = [0, 0, 0] while 1: cnt = 0 tstart = time() while (time() - tstart) < timeout: if self.port.inWaiting() > 0: data[2] = ord(self.port.read()) break if (time() - tstart) > timeout: raise Exception("Read timeout") if data[0] == 0x3E and data[1] == 0x3E: break data[0] = data[1] data[1] = data[2] for d in range(data[2] - ord("0")): data.append(ord(self.port.read())) # if self.from_update: # crc = self.calcChecksum(data) # if data[len(data)-1]!=(crc&0xFF) or data[len(data)-2]!=(crc>>8): # self.port.flushInput() # raise Exception("Checksum failure") # if self.port.inWaiting() > 0: # self.port.flushInput() # self.from_update=0 return data # Send a packet def sipSend(self, data): if self.serialReady: self.sendPacket(data) # Receive all waiting packets. # returns the number of packets read def sipRecv(self): iters = 0 if self.serialReady: while self.port.inWaiting() > 0: try: recv = self.recvPacket() except: print "no recv packet" # continue break iters += 1 self.parseData(recv) # 0x3s means sip packet # if (recv[3]&0xF0)==0x30: # self.parseSip(recv) # 0xF0 means io packet # elif recv[3]==0xF0: # self.parseIO(recv) return iters def parseData(self): # parse all data stall = 0 bump = 0 sonars = [-1, -1, -1, -1, -1, -1] # Right Sonar sonars[4] = float(self.Ultrasonic_rear_right) # if -0.5<self.prev_sonar[4]- sonars[4]<0.5: # sonars[4] = self.prev_sonar4 # else: # self.prev_sonar4=sonars[4] # print 'Sonar 1: ', sonars[0] # Right Front Sonar sonars[3] = float(self.Ultrasonic_right) # print 'Sonar 2: ', sonars[1] # Front Sonar sonars[2] = float(self.Ultrasonic_front) # print 'Sonar 3: ', sonars[2] # Left Front Sonar sonars[1] = float(self.Ultrasonic_left) # print 'Sonar 4: ', sonars[3 # Left Sonar sonars[0] = float(self.Ultrasonic_rear_left) # print 'Sonar 5: ', sonars[4] # Back Sonar sonars[5] = float(self.Ultrasonic_back) # print 'Sonar 6: ', sonars[5] self.pos_values[0], self.pos_values[1] = self.EKF.get_position() self.pos_values[2] = self.EKF.get_heading() self.pos_values[2] = self.pos_values[2] % (2 * pi) if self.pos_values[2] > 2 * pi: self.pos_values[2] -= 2 * pi elif self.pos_values[2] < 0: self.pos_values[2] += 2 * pi self.odpose.set((self.pos_values[0], self.pos_values[1], self.pos_values[2])) self.stalled.set((stall != 0x00) or (bump != 0x00)) storedsonars = self.storedsonars.get() for i in range(len(sonars)): if sonars[i] != -1: storedsonars[i] = sonars[i] * METER_SCALE self.sonarsChanged[i] = 1 else: self.sonarsChanged[i] = 0 self.storedsonars.set(storedsonars) # Send a packet and receive a SIP response from the robot def sipSendReceive(self, data): self.sipSend(data) self.sipRecv() # def cmdPulse(self): # self.sipSendReceive([ArcosCmd.PULSE]) # def cmdPulseSend(self): # self.sipSend([ArcosCmd.PULSE]) def cmdEnable(self, v): # self.sendPacket([ArcosCmd.ENABLE, ArgType.ARGINT, v, 0]) pass def cmdVel(self, v): absv = int(abs(v)) if v >= 0: data = [ArcosCmd.VEL, ArgType.ARGINT, absv & 0xFF, absv >> 8] else: data = [ArcosCmd.VEL, ArgType.ARGNINT, absv & 0xFF, absv >> 8] # self.sipSendReceive(data) self.sipSend(data) # def cmdVel2(self,l,r): # self.sipSendReceive([ArcosCmd.VEL2,ArgType.ARGINT,int(r),int(l)]) def cmdRvel(self, rv): absrv = abs(rv) if rv >= 0: data = [ArcosCmd.RVEL, ArgType.ARGINT, absrv & 0xFF, absrv >> 8] else: data = [ArcosCmd.RVEL, ArgType.ARGNINT, absrv & 0xFF, absrv >> 8] # self.sipSendReceive(data) self.sipSend(data) def cmdSay(self, note, duration): data = [ArcosCmd.SAY, ArgType.ARGSTR] for d in "%s,%s" % (note, duration): data.append(ord(d)) data.append(0)
class eBot(): def __init__(self): self.sonarValues = [0, 0, 0, 0, 0, 0] self.all_Values = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0] self.port = None self.serialReady = False self.ldrvalue = [0, 0] self.p_value = [0, 0] self.acc_values = [0, 0, 0, 0, 0, 0] self.pos_values = [0, 0, 0] self.EKF = Locator_EKF([0., 0.], 0.) self.offset_counter = 0 self.thread_flag = 0 self.heading = 0. self.offset_counter_iteration = 100 def destroy(self): """ Destructor function for eBot class. """ self.disconnect() self.sonarValues = None self.port = None self.serialReady = None def getOpenPorts(self): """ Windows only function: Obtains a list of tuples with eBot-relevant port number and description. :rtype: list :return: devicePorts: list of port numbers and descriptions of relevant serial devices. """ path = 'HARDWARE\\DEVICEMAP\\SERIALCOMM' key = winreg.OpenKey(winreg.HKEY_LOCAL_MACHINE, path) ports = [] #maximum 256 entries, will break anyways for i in range(256): try: val = winreg.EnumValue(key, i) port = (str(val[1]), str(val[0])) ports.append(port) except Exception: winreg.CloseKey(key) break #return ports #ports = getOpenPorts() devicePorts = [] for port in ports: #Just because it is formatted that way... if 'BthModem' in port[1][8:] or 'VCP' in port[1][ 8:] or 'ProlificSerial' in port[1][8:]: devicePorts.append((int(port[0][3:]) - 1)) return devicePorts def open(self): """ Opens connection with the eBot via BLE. Connects with the first eBot that the computer is paired to. :raise Exception: No eBot found """ self.connect() def connect(self): """ Opens connection with the eBot via BLE. Connects with the first eBot that the computer is paired to. :raise Exception: No eBot found """ baudRate = 115200 ports = [] if os.name == "posix": if sys.platform == "linux2": #usbSerial = glob.glob('/dev/ttyUSB*') ports = glob.glob('/dev/rfcomm*') #print "Support for this OS is under development." elif sys.platform == "darwin": ports = glob.glob('/dev/tty.eBo*') #usbSerial = glob.glob('/dev/tty.usbserial*') else: print "Unknown posix OS." sys.exit() elif os.name == "nt": ports = self.getOpenPorts() #ports = ['COM' + str(i + 1) for i in range(256)] #EBOT_PORTS = getEBotPorts() connect = 0 ebot_ports = [] ebot_names = [] line = "a" print "connecting", for port in ports: try: print ".", if (line[:2] == "eB"): break s = Serial(port, baudRate, timeout=5.0, writeTimeout=5.0) s._timeout = 5.0 s._writeTimeout = 5.0 #try: # s.open() #except: # continue while (line[:2] != "eB"): if (s.inWaiting() > 0): line = s.readline() s.write("<<1?") sleep(0.5) line = s.readline() if (line[:2] == "eB"): ebot_ports.append(port) ebot_names.append(line) connect = 1 self.port = s self.portName = port self.port._timeout = 5.0 self.port._writeTimeout = 5.0 self.port.flushInput() self.port.flushOutput() break #s.close() # self. except: try: if s.isOpen(): s.close() except: pass if (connect == 0): try: self.port.close() except: pass #sys.stderr.write("Could not open serial port. Is robot turned on and connected?\n") print "Connection Error", "No eBot found. Please reconnect and try again.", #import ctypes # An included library with Python install. #ctypes.windll.user32.MessageBoxA(0, "Your text", "Your title", 1) raise Exception("No eBot found") sleep(.01) try: self.port.write('<<1E') sleep(0.4) line = self.port.readline() if (line != ">>1B\n" and line != ">>1B" and line != ">>\n" and line != ">>"): self.lostConnection() self.port.write("<<1O") sleep(0.4) self.port.write("F") sleep(0.2) self.port.flushInput() self.port.flushOutput() print "connected" self.receive_thread = Thread(target=self.recieve_background) self.thread_flag = 1 self.receive_thread.start() self.offset_counter = 0 except: print "COM Error", "Robot connection lost..." sys.stderr.write("Could not write to serial port.\n") self.serialReady = False sys.stderr.write("Robot turned off or no longer connected.\n") sleep(7) self.serialReady = True def read(self): self.incoming = None if self.offset_counter > 1: self.prev_time_stamp = self.time_stamp if self.port.inWaiting() != 0: self.incoming = self.port.readline().rstrip('\n') self.offset_counter += 1 try: self.time_stamp , self.Ax , self.Ay , self.Az , self.Gx , self.Gy , self.Gz , \ self.Ultrasonic_rear_right, self.Ultrasonic_right, self.Ultrasonic_front , \ self.Ultrasonic_left , self.Ultrasonic_rear_left , self.Ultrasonic_back , \ self.encoder_right , self.encoder_left , self.LDR_top , self.LDR_front , \ self.tempreture_sensor , self.voltage , self.current = self.incoming.split(";") self.time_stamp = float(self.time_stamp) self.Ax = float(self.Ax) self.Ay = float(self.Ay) self.Az = float(self.Az) self.Gx = float(self.Gx) self.Gy = float(self.Gy) self.Gz = float(self.Gz) self.Ultrasonic_rear_right = float(self.Ultrasonic_rear_right) self.Ultrasonic_right = float(self.Ultrasonic_right) self.Ultrasonic_front = float(self.Ultrasonic_front) self.Ultrasonic_left = float(self.Ultrasonic_left) self.Ultrasonic_rear_left = float(self.Ultrasonic_rear_left) self.Ultrasonic_back = float(self.Ultrasonic_back) self.encoder_right = float(self.encoder_right) self.encoder_left = float(self.encoder_left) self.LDR_top = float(self.LDR_top) self.LDR_front = float(self.LDR_front) self.tempreture_sensor = float(self.tempreture_sensor) self.voltage = float(self.voltage) self.current = float(self.current) except: #print "eBot.read(): bad formatted data received" #print self.incoming pass if self.offset_counter == 2: self.Ax_offset = self.Ax self.Ay_offset = self.Ay self.Az_offset = self.Az self.Gx_offset = self.Gx self.Gy_offset = self.Gy self.Gz_offset = self.Gz if self.offset_counter > 2 and self.offset_counter < self.offset_counter_iteration: # we just average the first 50 # readings of the accelerometer and Gyro sensor and find the offset value by averaging them self.Ax_offset = (self.Ax + self.Ax_offset) self.Ay_offset = (self.Ay + self.Ay_offset) self.Az_offset = (self.Az + self.Az_offset) self.Gx_offset = (self.Gx + self.Gx_offset) self.Gy_offset = (self.Gy + self.Gy_offset) self.Gz_offset = (self.Gz + self.Gz_offset) if self.offset_counter == self.offset_counter_iteration: # we just average the first 50 # readings of the accelerometer and Gyro sensor and find the offset value by averaging them self.Ax_offset = self.Ax_offset / ( self.offset_counter_iteration - 2) self.Ay_offset = self.Ay_offset / ( self.offset_counter_iteration - 2) self.Az_offset = self.Az_offset / ( self.offset_counter_iteration - 2) self.Gx_offset = self.Gx_offset / ( self.offset_counter_iteration - 2) self.Gy_offset = self.Gy_offset / ( self.offset_counter_iteration - 2) self.Gz_offset = self.Gz_offset / ( self.offset_counter_iteration - 2) if self.offset_counter > self.offset_counter_iteration: sampling_time = (self.time_stamp - self.prev_time_stamp) / 1000. if sampling_time > 0: if abs(self.Gz - self.Gz_offset) > 50: # to remove the noise self.heading = self.heading + sampling_time * ( self.Gz - self.Gz_offset ) # the integration to get the heading self.heading_scaled = self.heading / 130.5 #128.7 self.pos_values[0],self.pos_values[1],self.pos_values[2] = \ self.EKF.update_state([self.heading_scaled*pi/180.,self.encoder_right/1000.,self.encoder_left/1000.],sampling_time) ## self.pos_values[2] = degrees(self.pos_values[2]) ## self.pos_values[2] = self.pos_values[2] % 360 ## if self.pos_values[2]>180: ## self.pos_values[2]-=360 ## elif self.pos_values[2]<-180: ## self.pos_values[2]+=360 return self.incoming def recieve_background(self): while self.thread_flag: self.read() sleep(0.005) return def close(self): """ Close BLE connection with eBot. """ self.disconnect() #TODO: add disconnect feedback to robot def disconnect(self): """ Close BLE connection with eBot. """ #self.receive_thread. sleep(0.05) self.thread_flag = 0 sleep(0.05) if self.serialReady: try: self.port.close() print "Successful", "eBot successfully disconnected." except: self.lostConnection() def sonars(self): """ Retrieves and returns all six ultrasonic sensor values from the eBot in meters. :rtype: list :return: sonarValues """ self.sonarValues[3] = float(self.Ultrasonic_rear_right) / 1000 self.sonarValues[4] = float(self.Ultrasonic_right) / 1000 self.sonarValues[2] = float(self.Ultrasonic_front) / 1000 self.sonarValues[1] = float(self.Ultrasonic_left) / 1000 self.sonarValues[0] = float(self.Ultrasonic_rear_left) / 1000 self.sonarValues[5] = float(self.Ultrasonic_back) / 1000 return self.sonarValues def calibration_values(self): """ Retrieves and returns the calibration values of the eBot. :rtype: list :return: all_Values (calibration values) """ if self.serialReady: try: self.port.write("2C") except: self.lostConnection() line = self.port.readline() values = line.split(";") while len(values) < 10: if self.serialReady: try: self.port.write("2C") except: self.lostConnection() line = self.port.readline() values = line.split(";") self.all_Values[0] = float(values[0]) self.all_Values[1] = float(values[1]) self.all_Values[2] = float(values[2]) self.all_Values[3] = float(values[3]) self.all_Values[8] = float(values[4]) / 1000 self.all_Values[7] = float(values[5]) / 1000 self.all_Values[6] = float(values[6]) / 1000 self.all_Values[5] = float(values[7]) / 1000 self.all_Values[4] = float(values[8]) / 1000 self.all_Values[9] = float(values[9]) / 1000 return self.all_Values def halt(self): """ Halts the eBot, turns the motors and LEDs off. """ if self.serialReady: try: self.port.write("2H") except: self.lostConnection() sleep(0.05) def led(self, bool): """ Controls the state of the LED on the eBot. :param bool: Defines whether the LED should turn ON (1) or OFF (0) """ if (bool == 1): self.led_on() elif (bool == 0): self.led_off() else: self.led_off() sleep(0.05) def led_on(self): """ Turns the LED on the eBot ON. """ if self.serialReady: try: self.port.write("2L") except: self.lostConnection() sleep(0.05) def led_off(self): """ Turns the LED on the eBot OFF. """ if self.serialReady: try: self.port.write("2l") except: self.lostConnection() sleep(0.05) def light(self): """ Retrieves and returns a list of tuples with the light index. 0 index is front and 1st index is top LDR readings. :rtype: list :return: ldrvalue: LDR Readings """ self.ldrvalue[0] = float(self.LDR_front) self.ldrvalue[1] = float(self.LDR_top) return self.ldrvalue #Double check true vs. false def obstacle(self): """ Tells whether or not there is an obstacle less than 250 mm away from the front of the eBot. :rtype: bool :return: True if obstacle exists """ if self.Ultrasonic_front > 250: return False else: return True #TODO: implement x, y, z returns and a seperate odometry function def acceleration(self): """ Retrieves and returns accelerometer values; absolute values of X,Y and theta coordinates of robot with reference to starting position. :rtype: list :return: acc_values: Accelerometer values """ self.acc_values[0] = float(self.Ax - self.Ax_offset) self.acc_values[1] = float(self.Ay - self.Ay_offset) self.acc_values[2] = float(self.Az - self.Az_offset) self.acc_values[3] = float(self.Gx - self.Gx_offset) self.acc_values[4] = float(self.Gy - self.Gy_offset) self.acc_values[5] = float(self.Gz - self.Gz_offset) return self.acc_values def odometry(self): """ Retrieves and returns the odometry values of the eBot as a Pose object with respect to the robot initial position. Pose.x: x coordinate in meters Pose.y: y coordinate in meters Pose.theta: rotation in radians :rtype: Pose object :return: Pose: Represent the x, y, theta pose of an object in 2D space """ self.pos_values[0], self.pos_values[1] = self.EKF.get_position() self.pos_values[2] = self.EKF.get_heading() self.pos_values[2] = self.pos_values[2] % (2 * pi) if self.pos_values[2] > 2 * pi: self.pos_values[2] -= 2 * pi elif self.pos_values[2] < 0: self.pos_values[2] += 2 * pi pose = (self.pos_values[0], self.pos_values[1], self.pos_values[2]) return util.valueListToPose(pose) #TODO: implement temperature feedback from MPU6050 IC def temperature(self): """ Retrieves and returns temperature reading from the eBot. :rtype: int :return: Temperature value. """ return int(self.tempreture_sensor) def power(self): """ :return: """ self.p_value[0] = float(self.voltage) self.p_value[1] = float(self.current) return self.p_value def imperial_march(self): """ """ if self.serialReady: try: self.port.write("2b") except: self.lostConnection() def buzzer(self, btime, bfreq): """ Plays the buzzer for given time at given frequency. :param btime: Time in Seconds :param bfreq: Frequency in Hertz """ buzzer_time = int(btime) buzzer_frequency = int(bfreq) bt1 = str(buzzer_time) bf1 = str(buzzer_frequency) str_len = len(bt1) + len(bf1) + 2 str_len = str_len + 48 myvalue = chr(str_len) + 'B' + bt1 + ';' + bf1 if self.serialReady: try: self.port.write(myvalue) except: self.lostConnection() sleep(buzzer_time / 1000) def port_name(self): """ Returns port name of currently connected eBot. :return: port: Port name """ return self.port def port_close(self): """ Closes the COM port that corresponds to the eBot object. :raise Exception: Could not close COM port """ try: self.port.close() except: self.serialReady = False raise Exception("Could not close COM port.") #TODO: Add com port argument functionality def port_open(self): """ Still under development, currently just calls connect """ self.connect() def wheels(self, LS, RS): """ Controls the speed of the wheels of the robot according to the specified values :param LS: Speed of left motor :param RS: Speed of right motor """ if LS > 1: LS = 1 elif LS < -1: LS = -1 if RS > 1: RS = 1 elif RS < -1: RS = -1 Left_speed = int((LS + 2) * 100) Right_speed = int((RS + 2) * 100) LS1 = str(Left_speed) RS1 = str(Right_speed) myvalue = '8' + 'w' + LS1 + ';' + RS1 if self.serialReady: try: self.port.write(myvalue) except: self.lostConnection() # class ebot_f: # def __init__(self): sleep(0.05) def wheel_calibrate(self, LS, RS): """ Controls the speed of the wheels of the robot according to the specified values :param LS: Speed of left motor :param RS: Speed of right motor """ if LS > 9999: LS = 9999 elif LS < 1: LS = 1 if RS > 9999: RS = 9999 elif RS < 1: RS = 1 LS1 = str(LS).zfill(4) RS1 = str(RS).zfill(4) myvalue = ':' + 'c' + LS1 + ';' + RS1 if self.serialReady: try: self.port.write(myvalue) except: self.lostConnection() # class ebot_f: # def __init__(self) def lostConnection(self): """ Handler for the case that the computer loses connection with the eBot. :raise Exception: Robot Connection Lost """ try: self.port.close() except: pass self.serialReady = False print "COM Error", "Robot connection lost..." raise Exception("Robot Connection Lost")
class eBot(): def __init__(self): self.sonarValues = [0, 0, 0, 0, 0, 0] self.all_Values = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0] self.port = None self.serialReady = False self.ldrvalue = [0, 0] self.p_value = [0, 0] self.acc_values = [0, 0, 0, 0, 0, 0] self.pos_values = [0, 0, 0] self.EKF = Locator_EKF([0.,0.],0.) self.offset_counter = 0 self.thread_flag = 0 self.heading =0. self.offset_counter_iteration = 100 def destroy(self): """ Destructor function for eBot class. """ self.disconnect() self.sonarValues = None self.port = None self.serialReady = None def getOpenPorts(self): """ Windows only function: Obtains a list of tuples with eBot-relevant port number and description. :rtype: list :return: devicePorts: list of port numbers and descriptions of relevant serial devices. """ path = 'HARDWARE\\DEVICEMAP\\SERIALCOMM' key = winreg.OpenKey(winreg.HKEY_LOCAL_MACHINE, path) ports = [] #maximum 256 entries, will break anyways for i in range(256): try: val = winreg.EnumValue(key, i) port = (str(val[1]), str(val[0])) ports.append(port) except Exception: winreg.CloseKey(key) break #return ports #ports = getOpenPorts() devicePorts = [] for port in ports: #Just because it is formatted that way... if 'BthModem' in port[1][8:] or 'VCP' in port[1][8:] or 'ProlificSerial' in port[1][8:]: devicePorts.append((int(port[0][3:]) - 1)) return devicePorts def open(self): """ Opens connection with the eBot via BLE. Connects with the first eBot that the computer is paired to. :raise Exception: No eBot found """ self.connect() def connect(self): """ Opens connection with the eBot via BLE. Connects with the first eBot that the computer is paired to. :raise Exception: No eBot found """ baudRate = 115200 ports = [] if os.name == "posix": if sys.platform == "linux2": #usbSerial = glob.glob('/dev/ttyUSB*') ports = glob.glob('/dev/rfcomm*') #print "Support for this OS is under development." elif sys.platform == "darwin": ports = glob.glob('/dev/tty.eBo*') #usbSerial = glob.glob('/dev/tty.usbserial*') else: print "Unknown posix OS." sys.exit() elif os.name == "nt": ports = self.getOpenPorts() #ports = ['COM' + str(i + 1) for i in range(256)] #EBOT_PORTS = getEBotPorts() connect = 0 ebot_ports = [] ebot_names = [] line = "a" print "connecting", for port in ports: try: print ".", if (line[:2] == "eB"): break s = Serial(port, baudRate, timeout=5.0, writeTimeout=5.0) s._timeout = 5.0 s._writeTimeout = 5.0 #try: # s.open() #except: # continue while (line[:2] != "eB"): if (s.inWaiting()>0): line=s.readline() s.write("<<1?") sleep(0.5) line = s.readline() if (line[:2] == "eB"): ebot_ports.append(port) ebot_names.append(line) connect = 1 self.port = s self.portName = port self.port._timeout = 5.0 self.port._writeTimeout = 5.0 self.port.flushInput() self.port.flushOutput() break #s.close() # self. except: try: if s.isOpen(): s.close() except: pass if (connect == 0): try: self.port.close() except: pass #sys.stderr.write("Could not open serial port. Is robot turned on and connected?\n") print "Connection Error", "No eBot found. Please reconnect and try again.", #import ctypes # An included library with Python install. #ctypes.windll.user32.MessageBoxA(0, "Your text", "Your title", 1) raise Exception("No eBot found") sleep(.01) try: self.port.write('<<1E') sleep(0.4) line = self.port.readline() if (line != ">>1B\n" and line != ">>1B" and line != ">>\n" and line != ">>"): self.lostConnection() self.port.write("<<1O") sleep(0.4) self.port.write("F") sleep(0.2) self.port.flushInput() self.port.flushOutput() print "connected" self.receive_thread = Thread(target= self.recieve_background) self.thread_flag =1 self.receive_thread.start() self.offset_counter =0 except: print "COM Error", "Robot connection lost..." sys.stderr.write("Could not write to serial port.\n") self.serialReady = False sys.stderr.write("Robot turned off or no longer connected.\n") sleep(7) self.serialReady = True def read(self): self.incoming=None if self.offset_counter>1: self.prev_time_stamp = self.time_stamp if self.port.inWaiting()!=0: self.incoming=self.port.readline().rstrip('\n') self.offset_counter += 1 try: self.time_stamp , self.Ax , self.Ay , self.Az , self.Gx , self.Gy , self.Gz , \ self.Ultrasonic_rear_right, self.Ultrasonic_right, self.Ultrasonic_front , \ self.Ultrasonic_left , self.Ultrasonic_rear_left , self.Ultrasonic_back , \ self.encoder_right , self.encoder_left , self.LDR_top , self.LDR_front , \ self.tempreture_sensor , self.voltage , self.current = self.incoming.split(";") self.time_stamp = float(self.time_stamp) self.Ax = float(self.Ax) self.Ay = float(self.Ay) self.Az = float(self.Az) self.Gx = float(self.Gx) self.Gy = float(self.Gy) self.Gz = float(self.Gz) self.Ultrasonic_rear_right = float(self.Ultrasonic_rear_right) self.Ultrasonic_right = float(self.Ultrasonic_right) self.Ultrasonic_front = float(self.Ultrasonic_front) self.Ultrasonic_left = float(self.Ultrasonic_left) self.Ultrasonic_rear_left = float(self.Ultrasonic_rear_left) self.Ultrasonic_back = float(self.Ultrasonic_back) self.encoder_right = float(self.encoder_right) self.encoder_left = float(self.encoder_left) self.LDR_top = float(self.LDR_top) self.LDR_front = float(self.LDR_front) self.tempreture_sensor = float(self.tempreture_sensor) self.voltage = float(self.voltage) self.current = float(self.current) except: #print "eBot.read(): bad formatted data received" #print self.incoming pass if self.offset_counter == 2: self.Ax_offset = self.Ax self.Ay_offset = self.Ay self.Az_offset = self.Az self.Gx_offset = self.Gx self.Gy_offset = self.Gy self.Gz_offset = self.Gz if self.offset_counter>2 and self.offset_counter<self.offset_counter_iteration:# we just average the first 50 # readings of the accelerometer and Gyro sensor and find the offset value by averaging them self.Ax_offset = (self.Ax+self.Ax_offset) self.Ay_offset = (self.Ay+self.Ay_offset) self.Az_offset = (self.Az+self.Az_offset) self.Gx_offset = (self.Gx+self.Gx_offset) self.Gy_offset = (self.Gy+self.Gy_offset) self.Gz_offset = (self.Gz+self.Gz_offset) if self.offset_counter == self.offset_counter_iteration:# we just average the first 50 # readings of the accelerometer and Gyro sensor and find the offset value by averaging them self.Ax_offset = self.Ax_offset/(self.offset_counter_iteration-2) self.Ay_offset = self.Ay_offset/(self.offset_counter_iteration-2) self.Az_offset = self.Az_offset/(self.offset_counter_iteration-2) self.Gx_offset = self.Gx_offset/(self.offset_counter_iteration-2) self.Gy_offset = self.Gy_offset/(self.offset_counter_iteration-2) self.Gz_offset = self.Gz_offset/(self.offset_counter_iteration-2) if self.offset_counter>self.offset_counter_iteration: sampling_time = (self.time_stamp-self.prev_time_stamp)/1000. if sampling_time>0: if abs(self.Gz-self.Gz_offset)>50: # to remove the noise self.heading = self.heading+sampling_time*(self.Gz-self.Gz_offset) # the integration to get the heading self.heading_scaled = self.heading/130.5 #128.7 self.pos_values[0],self.pos_values[1],self.pos_values[2] = \ self.EKF.update_state([self.heading_scaled*pi/180.,self.encoder_right/1000.,self.encoder_left/1000.],sampling_time) ## self.pos_values[2] = degrees(self.pos_values[2]) ## self.pos_values[2] = self.pos_values[2] % 360 ## if self.pos_values[2]>180: ## self.pos_values[2]-=360 ## elif self.pos_values[2]<-180: ## self.pos_values[2]+=360 return self.incoming def recieve_background(self): while self.thread_flag: self.read() sleep(0.005) return def close(self): """ Close BLE connection with eBot. """ self.disconnect() #TODO: add disconnect feedback to robot def disconnect(self): """ Close BLE connection with eBot. """ #self.receive_thread. sleep(0.05) self.thread_flag = 0 sleep(0.05) if self.serialReady: try: self.port.close() print "Successful", "eBot successfully disconnected." except: self.lostConnection() def sonars(self): """ Retrieves and returns all six ultrasonic sensor values from the eBot in meters. :rtype: list :return: sonarValues """ self.sonarValues[3] = float(self.Ultrasonic_rear_right) / 1000 self.sonarValues[4] = float(self.Ultrasonic_right) / 1000 self.sonarValues[2] = float(self.Ultrasonic_front) / 1000 self.sonarValues[1] = float(self.Ultrasonic_left) / 1000 self.sonarValues[0] = float(self.Ultrasonic_rear_left) / 1000 self.sonarValues[5] = float(self.Ultrasonic_back) / 1000 return self.sonarValues def calibration_values(self): """ Retrieves and returns the calibration values of the eBot. :rtype: list :return: all_Values (calibration values) """ if self.serialReady: try: self.port.write("2C") except: self.lostConnection() line = self.port.readline() values = line.split(";") while len(values) < 10: if self.serialReady: try: self.port.write("2C") except: self.lostConnection() line = self.port.readline() values = line.split(";") self.all_Values[0] = float(values[0]) self.all_Values[1] = float(values[1]) self.all_Values[2] = float(values[2]) self.all_Values[3] = float(values[3]) self.all_Values[8] = float(values[4]) / 1000 self.all_Values[7] = float(values[5]) / 1000 self.all_Values[6] = float(values[6]) / 1000 self.all_Values[5] = float(values[7]) / 1000 self.all_Values[4] = float(values[8]) / 1000 self.all_Values[9] = float(values[9]) / 1000 return self.all_Values def halt(self): """ Halts the eBot, turns the motors and LEDs off. """ if self.serialReady: try: self.port.write("2H") except: self.lostConnection() sleep(0.05) def led(self, bool): """ Controls the state of the LED on the eBot. :param bool: Defines whether the LED should turn ON (1) or OFF (0) """ if (bool == 1): self.led_on() elif (bool == 0): self.led_off() else: self.led_off() sleep(0.05) def led_on(self): """ Turns the LED on the eBot ON. """ if self.serialReady: try: self.port.write("2L") except: self.lostConnection() sleep(0.05) def led_off(self): """ Turns the LED on the eBot OFF. """ if self.serialReady: try: self.port.write("2l") except: self.lostConnection() sleep(0.05) def light(self): """ Retrieves and returns a list of tuples with the light index. 0 index is front and 1st index is top LDR readings. :rtype: list :return: ldrvalue: LDR Readings """ self.ldrvalue[0] = float(self.LDR_front) self.ldrvalue[1] = float(self.LDR_top) return self.ldrvalue #Double check true vs. false def obstacle(self): """ Tells whether or not there is an obstacle less than 250 mm away from the front of the eBot. :rtype: bool :return: True if obstacle exists """ if self.Ultrasonic_front>250: return False else: return True #TODO: implement x, y, z returns and a seperate odometry function def acceleration(self): """ Retrieves and returns accelerometer values; absolute values of X,Y and theta coordinates of robot with reference to starting position. :rtype: list :return: acc_values: Accelerometer values """ self.acc_values[0] = float(self.Ax-self.Ax_offset) self.acc_values[1] = float(self.Ay-self.Ay_offset) self.acc_values[2] = float(self.Az-self.Az_offset) self.acc_values[3] = float(self.Gx-self.Gx_offset) self.acc_values[4] = float(self.Gy-self.Gy_offset) self.acc_values[5] = float(self.Gz-self.Gz_offset) return self.acc_values def odometry(self): """ Retrieves and returns the odometry values of the eBot as a Pose object with respect to the robot initial position. Pose.x: x coordinate in meters Pose.y: y coordinate in meters Pose.theta: rotation in radians :rtype: Pose object :return: Pose: Represent the x, y, theta pose of an object in 2D space """ self.pos_values[0] , self.pos_values[1] = self.EKF.get_position() self.pos_values[2] = self.EKF.get_heading() self.pos_values[2] =self.pos_values[2] % (2*pi) if self.pos_values[2]>2*pi: self.pos_values[2]-=2*pi elif self.pos_values[2]<0: self.pos_values[2]+=2*pi pose = (self.pos_values[0],self.pos_values[1],self.pos_values[2]) return util.valueListToPose(pose) #TODO: implement temperature feedback from MPU6050 IC def temperature(self): """ Retrieves and returns temperature reading from the eBot. :rtype: int :return: Temperature value. """ return int(self.tempreture_sensor) def power(self): """ :return: """ self.p_value[0] = float(self.voltage) self.p_value[1] = float(self.current) return self.p_value def imperial_march(self): """ """ if self.serialReady: try: self.port.write("2b") except: self.lostConnection() def buzzer(self, btime, bfreq): """ Plays the buzzer for given time at given frequency. :param btime: Time in Seconds :param bfreq: Frequency in Hertz """ buzzer_time = int(btime) buzzer_frequency = int(bfreq) bt1 = str(buzzer_time) bf1 = str(buzzer_frequency) str_len = len(bt1) + len(bf1)+2 str_len =str_len + 48 myvalue = chr(str_len) + 'B' + bt1 + ';' + bf1 if self.serialReady: try: self.port.write(myvalue) except: self.lostConnection() sleep(buzzer_time/1000) def port_name(self): """ Returns port name of currently connected eBot. :return: port: Port name """ return self.port def port_close(self): """ Closes the COM port that corresponds to the eBot object. :raise Exception: Could not close COM port """ try: self.port.close() except: self.serialReady = False raise Exception("Could not close COM port.") #TODO: Add com port argument functionality def port_open(self): """ Still under development, currently just calls connect """ self.connect() def wheels(self, LS, RS): """ Controls the speed of the wheels of the robot according to the specified values :param LS: Speed of left motor :param RS: Speed of right motor """ if LS > 1: LS = 1 elif LS < -1: LS = -1 if RS > 1: RS = 1 elif RS < -1: RS = -1 Left_speed = int((LS + 2) * 100) Right_speed = int((RS + 2) * 100) LS1 = str(Left_speed) RS1 = str(Right_speed) myvalue = '8' + 'w' + LS1 + ';' + RS1 if self.serialReady: try: self.port.write(myvalue) except: self.lostConnection() # class ebot_f: # def __init__(self): sleep(0.05) def wheel_calibrate(self, LS, RS): """ Controls the speed of the wheels of the robot according to the specified values :param LS: Speed of left motor :param RS: Speed of right motor """ if LS > 9999: LS = 9999 elif LS < 1: LS = 1 if RS > 9999: RS = 9999 elif RS < 1: RS = 1 LS1 = str(LS).zfill(4) RS1 = str(RS).zfill(4) myvalue = ':' + 'c' + LS1 + ';' + RS1 if self.serialReady: try: self.port.write(myvalue) except: self.lostConnection() # class ebot_f: # def __init__(self) def lostConnection(self): """ Handler for the case that the computer loses connection with the eBot. :raise Exception: Robot Connection Lost """ try: self.port.close() except: pass self.serialReady = False print "COM Error", "Robot connection lost..." raise Exception("Robot Connection Lost")