import serial import time, gc import termios, fcntl, sys, os gc.collect() def combine(high, low): return (high << 8) | low def getBit(bit, byte): return (byte >> bit) & 0x01 def splitWideInt(num): if num >= 0: out = num else: out = (1<<16) + num return ( (out >> 8) & 0xff, out & 0xff) class robot(): def __init__(self): self.port = serial.Serial('/dev/tty.ElementSerial-ElementSe-1', 57600) self.restart() self.sensors = {} self.sendBytes([140, 0, 6, 48, 32, 45, 32]) time.sleep(0.1) self.sendBytes([140, 1, 6, 72, 20, 76, 20, 79, 20]) def __del__(self): print 'destroying' self.port.close() def sendBytes(self, bytes): out_string = '' for i in range(len(bytes)): out_string += chr(bytes[i]) self.port.write(out_string) def restart(self): self.port.setTimeout(0.5) self.init() time.sleep(0.2) self.safeMode() def close(self): self.port.close() def init(self): self.sendBytes([128]) def safeMode(self): self.sendBytes([131]) def drive(self, vel, rad): vel = splitWideInt(vel); rad = splitWideInt(rad); self.sendBytes([137, vel[0], vel[1], rad[0], rad[1]]) def straight(self, vel): self.drive(vel, 32768) def waitDistance(self, vel): vel = splitWideInt(vel) self.sendBytes([156, vel[0], vel[1]]) def waitAngle(self, vel): vel = splitWideInt(vel) self.sendBytes([157, vel[0], vel[1]]) self.sendBytes def turn(self, vel): self.drive(vel, 1) def stop(self): self.drive(0,0) def remote(self): fd = sys.stdin.fileno() oldterm = termios.tcgetattr(fd) newattr = termios.tcgetattr(fd) newattr[3] = newattr[3] & ~termios.ICANON & ~termios.ECHO termios.tcsetattr(fd, termios.TCSANOW, newattr) oldflags = fcntl.fcntl(fd, fcntl.F_GETFL) fcntl.fcntl(fd, fcntl.F_SETFL, oldflags | os.O_NONBLOCK) try: while 1: try: c = sys.stdin.read(1) if c == 'q': self.stop() return speed = 150 if c == 's': self.turn(speed) if c == 'f': self.turn(-speed) if c == 'e': self.straight(speed) if c == 'd': self.straight(-speed) if c == ' ': self.stop() except IOError: pass finally: termios.tcsetattr(fd, termios.TCSAFLUSH, oldterm) fcntl.fcntl(fd, fcntl.F_SETFL, oldflags) def playSong(self, num): self.sendBytes([141, num]) def parseSensorData(self, data): try: self.sensors['bumpRight'] = getBit(0, data[0]) self.sensors['bumpLeft'] = getBit(1, data[0]) self.sensors['dropCaster'] = getBit(4, data[0]) self.sensors['dropLeft'] = getBit(3, data[0]) self.sensors['dropRight'] = getBit(2, data[0]) self.sensors['wall'] = getBit(0, data[1]) self.sensors['infrared'] = data[10] self.sensors['chargingState'] = data[16] self.sensors['wallSignal'] = combine(data[26], data[27]) self.sensors['chargingSource'] = data[39] self.sensors['oiMode'] = data[40] except: print 'Error reading sensors! Data packet size was ' + str(len(data)) def getSensor(self): s = 52 self.sendBytes([142, 6]) time.sleep(0.1) vals = self.port.read(size=s) vals = [ord(c) for c in vals] self.parseSensorData(vals) def inSingleBeam(self, dir): if self.sensors['bumpRight'] or self.sensors['bumpLeft']: self.backupTryagain() else : self.drive(100, -300*dir) time.sleep(0.1) def fastTurnAround(self, orgDir): dir = orgDir if self.sensors['bumpLeft']: dir = -1 if self.sensors['bumpRight'] or self.sensors['bumpLeft']: self.straight(-250*dir) time.sleep(0.3) self.turn(200*dir) time.sleep(0.6) self.drive(250, -300*dir) time.sleep(0.5) self.turn(-200*dir) time.sleep(0.8) self.drive(150, -300*dir) time.sleep(0.2) def backupTryagain(self): self.straight(-250) time.sleep(0.3) self.turn(200) time.sleep(0.3) self.drive(150, -900) def backupTurnGoStraight(self, dir): self.straight(-250*dir) time.sleep(0.4) self.turn(200*dir) time.sleep(0.3) self.drive(150, -900*dir) time.sleep(0.2) self.turn(-200*dir) time.sleep(0.3) def singleBeamForce(self, dir): self.drive(100, 100*dir) time.sleep(0.1) if self.sensors['bumpRight'] or self.sensors['bumpLeft']: self.backupTurnGoStraight(dir) def auto(self): self.getSensor() self.drive(150, -500) self.wasAtWall = False self.wasAtRGF = False while 1: self.getSensor() #print self.sensors #continue if self.sensors['infrared'] == 248: print 'red :-)' self.inSingleBeam(1) self.wasAtRGF = False elif self.sensors['infrared'] == 244: print 'green :-)' self.inSingleBeam(-1) self.wasAtRGF = False elif self.sensors['infrared'] == 242: print 'force' if self.wasAtRGF: print 'At home!' self.stop() self.playSong(1) return if self.sensors['bumpRight'] or self.sensors['bumpLeft']: self.backupTryagain() elif self.sensors['infrared'] == 250: print 'red + force :-|' self.singleBeamForce(1) self.wasAtRGF = False elif self.sensors['infrared'] == 246: print 'green + force :-|' self.singleBeamForce(-1) self.wasAtRGF = False elif self.sensors['infrared'] == 254: print 'red + green + force 8-D' self.wasAtRGF = True self.straight(150) elif self.sensors['infrared'] == 252: print 'red and green :-D' self.straight(100) self.wasAtRGF = False elif self.sensors['wall'] and self.sensors['bumpRight']: print 'bump and wall' self.drive(100, 300) time.sleep(0.1) self.drive(150, -1500) self.wasAtWall = True elif self.sensors['bumpRight'] or self.sensors['bumpLeft']: print 'bumped!' self.backupTryagain() elif self.sensors['wall']: print 'at wall :-)' time.sleep(0.2) self.straight(150) self.wasAtWall = True elif self.sensors['wall'] == 0 and self.wasAtWall: print 'was at wall :-(' self.drive(150, -500) time.sleep(0.2) self.wasAtWall = False elif self.sensors['wall'] == 0: self.wasAtWall = False r=robot() r.auto()