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])
