def open(self, Serial): #baudRates = [9600,38400,19200,115200,57600] #baudRates = [115200,9600]#,38400,19200,57600] baudRates = [9600,19200,38400,57600,115200] #baudRates = [9600] curBaudRate = 0 self.port = Serial(self.portName,baudRates[0]) self.port._timeout = 1.0 self.port._writeTimout = 1.0 self.port.flushInput() self.port.flushOutput() numAttempts = 3 timeout = 5.0 startt = time() while (self.connectionState != STATE_READY): if (self.connectionState == STATE_NO_SYNC): self.sendPacket([CMD_SYNC0]) elif (self.connectionState == STATE_FIRST_SYNC): self.sendPacket([CMD_SYNC1]) elif (self.connectionState == STATE_SECOND_SYNC): self.sendPacket([CMD_SYNC2]) elif (self.connectionState == STATE_READY): pass try: pkt = self.recvPacket() except: if (self.connectionState == STATE_NO_SYNC and numAttempts >= 0): numAttempts -= 1 else: curBaudRate += 1 if (curBaudRate<len(baudRates)): self.port.close() self.port = Serial(self.portName,baudRates[curBaudRate]) debug("Changing to baud rate: "+`baudRates[curBaudRate]`, 1) self.port.flushInput() self.port.flushOutput() 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_SYNC0): self.connectionState = STATE_FIRST_SYNC elif (pkt[3]==CMD_SYNC1): self.connectionState = STATE_SECOND_SYNC elif (pkt[3]==CMD_SYNC2): 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") botName = "" botType = "" botSubType = "" i=4 while (pkt[i]): botName += chr(pkt[i]) i += 1 i+=1 while (pkt[i]): botType += chr(pkt[i]) i += 1 i+=1 while (pkt[i]): botSubType += chr(pkt[i]) i += 1 self.sendPacket([ArcosCmd.OPEN]) debug("P2OS Interface Ready - connected to "+`botName`+" "+`botType`+" "+`botSubType`,1) print "P2OS Interface Ready - connected to "+`botType`+`botName` changed = [0,0,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])