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