00001 """This module provides a class wrapping an iRobot Create."""
00002
00003 from serial import Serial
00004 from struct import pack
00005 from struct import unpack
00006 from time import sleep
00007 from threading import Thread
00008 from threading import Lock
00009 from datetime import datetime
00010
00011 class Monitor(Thread):
00012 def __init__(self, watchdog, packetRef, create, read, sendAll, addDistance, addAngle, update):
00013 Thread.__init__(self)
00014 self.watchdog = watchdog
00015 self.packetRef = packetRef
00016 self.create = create
00017 self.read = read
00018 self.sendAll = sendAll
00019 self.addDistance = addDistance
00020 self.addAngle = addAngle
00021 self.update = update
00022
00023 def mask(num):
00024 return (
00025 num >> 7 & 1,
00026 num >> 6 & 1,
00027 num >> 5 & 1,
00028 num >> 4 & 1,
00029 num >> 3 & 1,
00030 num >> 2 & 1,
00031 num >> 1 & 1,
00032 num & 1
00033 )
00034
00035 def unsigned(num):
00036 return (num,)
00037
00038 def signed(num):
00039 if (num >> 15):
00040 return (-1*(2**16-num),)
00041 return (num,)
00042
00043 def signed8(num):
00044 if (num >> 7):
00045 return (-1*(2**8-num),)
00046 return (num,)
00047
00048 self.packets = (
00049 (7,1,mask,("na","na","na","wheeldropCaster","wheeldropLeft","wheeldropRight","bumpLeft","bumpRight")),
00050 (8,1,unsigned,("wall",)),
00051 (9,1,unsigned,("cliffLeft",)),
00052 (10,1,unsigned,("cliffFronLeft",)),
00053 (11,1,unsigned,("cliffFrontRight",)),
00054 (12,1,unsigned,("cliffRight",)),
00055 (13,1,unsigned,("virtualWall",)),
00056 (17,1,unsigned,("infraredByte",)),
00057 (18,1,mask,("na","na","na","na","na","advance","na","play")),
00058 (19,2,signed,("distance",)),
00059 (20,2,signed,("angle",)),
00060 (21,1,unsigned,("chargingState",)),
00061 (22,2,unsigned,("voltage",)),
00062 (23,2,signed,("current",)),
00063 (24,1,signed8,("batteryTemperature",)),
00064 (25,2,unsigned,("batteryCharge",)),
00065 (26,2,unsigned,("batteryCapacity",)),
00066 (27,2,unsigned,("wallSignal",)),
00067 (28,2,unsigned,("cliffLeftSignal",)),
00068 (29,2,unsigned,("cliffFrontLeftSignal",)),
00069 (30,2,unsigned,("cliffFrontRightSignal",)),
00070 (31,2,unsigned,("cliffRightSignal",)),
00071 (34,1,mask,("na","na","na","na","na","na","homeBase","internalCharger")),
00072 (36,1,unsigned,("songNumber",)),
00073 (37,1,unsigned,("songPlaying",)),
00074 )
00075
00076 def run(self):
00077 while(len(self.watchdog) == 0):
00078 then = datetime.now()
00079 self.sendAll()
00080
00081 self.create.send(149,len(self.packets),*[i[0] for i in self.packets])
00082 self.sendAll()
00083
00084 bytes = self.read(sum([i[1] for i in self.packets]))
00085
00086 try:
00087 bytes = unpack('B'*sum([i[1] for i in self.packets]),bytes)
00088 data = {}
00089
00090 offset = 0
00091 for packet in self.packets:
00092 if (packet[1] == 1):
00093 results = packet[2](bytes[offset])
00094 else:
00095 results = packet[2]((bytes[offset] << 8) | (bytes[offset+1]))
00096
00097 index = 0
00098 for result in results:
00099 data[packet[3][index]] = result
00100 if (packet[3][index] == "distance"):
00101 data['d_distance'] = result
00102 data[packet[3][index]] = self.addDistance(result)
00103 if (packet[3][index] == "angle"):
00104 data['d_angle'] = result
00105 data[packet[3][index]] = self.addAngle(result)
00106 index = index + 1
00107
00108 offset = offset + packet[1]
00109
00110 if (len(self.packetRef)):
00111 self.packetRef[0] = data
00112 else:
00113 self.packetRef.append(data)
00114 except:
00115 print "bad data"
00116
00117 self.update()
00118
00119 now = datetime.now()
00120 elapsed = now - then
00121 elapsed = elapsed.seconds*1000. + elapsed.microseconds/1000.
00122 if (elapsed < self.create.period*1000.):
00123 sleep((self.create.period*1000. - elapsed)/1000.)
00124
00125 class Create:
00126 """Wrapper class for the iRobot Create"""
00127
00128 def __init__(self, tty="/dev/ttyUSB0"):
00129 """constructor for the Create, takes in a single argument: the serial port"""
00130
00131 self.timeout = 5
00132 self.period = .07
00133 self.runRef = []
00134 self.packetRef = []
00135 self.queueLock = Lock()
00136 self.queue = []
00137 self.distanceLock = Lock()
00138 self.__distance = 0
00139 self.angleLock = Lock()
00140 self.__angle = 0
00141 self.port = Serial(tty, 57600, timeout= self.timeout)
00142 self.portLock = Lock()
00143 self.update = lambda : ''
00144 self.reset()
00145
00146 def start(self):
00147 """Start the iCreate after initialization or reset."""
00148
00149 self.__sendNow(128,128,132,140,1,5,64,16,69,16,74,16,72,40,69,60,141,1)
00150 sleep(1)
00151 self.send(139,10,0,255)
00152
00153 monitor = Monitor(self.runRef, self.packetRef, self, self.__read, self.__sendAll, self.__addDistance, self.__addAngle, self.update)
00154 monitor.start()
00155 sleep(1.5)
00156
00157 def stop(self):
00158 """Stop the iCreate. Must be called before deletion of the iCreate object."""
00159 self.runRef.append('quit')
00160 sleep(1)
00161 rh,rl = self.__convert(0)
00162 lh,ll = self.__convert(0)
00163 self.__sendNow(145,rh,rl,lh,ll)
00164 self.__sendNow(139,0,0,255)
00165
00166 def reset(self):
00167 """Reset the iCreate."""
00168 self.runRef.append('quit')
00169 self.runRef = []
00170 sleep(1)
00171
00172 self.port.flushOutput()
00173 self.port.flushInput()
00174 self.__sendNow(128,7)
00175 self.__read(128,True)
00176
00177 self.port.flushInput()
00178
00179 def __convert(self, num):
00180 return self.__highLow(self.__twos(num))
00181
00182 def __highLow(self, num):
00183 return num >> 8, num & 0xFF
00184
00185 def __twos(self, num, bits=16):
00186 if (num >=0):
00187 return num
00188 return 2**bits+num
00189
00190 def send(self, *opcodes):
00191 self.queueLock.acquire()
00192
00193 def lmbda():
00194 self.__sendNow(*opcodes)
00195
00196 self.queue.append(lmbda)
00197
00198 self.queueLock.release()
00199
00200 def __sendNow(self, *opcodes):
00201 self.portLock.acquire()
00202 format = "B"*len(opcodes)
00203 data = pack(format, *opcodes)
00204 self.port.write(data)
00205 self.portLock.release()
00206
00207 def __sendAll(self):
00208 self.queueLock.acquire()
00209
00210 self.__read(self.port.inWaiting())
00211 for send in self.queue:
00212 send()
00213 self.queue = []
00214
00215 self.queueLock.release()
00216
00217 def __read(self,num,block=False):
00218 self.portLock.acquire()
00219 if (block):
00220 self.port.timeout = None
00221 bytes = self.port.read(num)
00222 if (block):
00223 self.port.timeout = self.timeout
00224 self.portLock.release()
00225 return bytes
00226
00227 def __getattr__(self,name):
00228 if (len(self.packetRef)):
00229 if (name in self.packetRef[0]):
00230 return self.packetRef[0][name]
00231 raise AttributeError, "no %s attribute" % (name)
00232
00233 def __addDistance(self,num):
00234 self.distanceLock.acquire()
00235 self.__distance = self.__distance + num
00236 dis = self.__distance
00237 self.distanceLock.release()
00238 return dis
00239
00240 def __addAngle(self,num):
00241 self.angleLock.acquire()
00242 self.__angle = self.__angle + num
00243 ang = self.__angle
00244 self.angleLock.release()
00245 return ang
00246
00247 def clearDistance(self):
00248 self.distanceLock.acquire()
00249 self.__distance = 0
00250 self.distanceLock.release()
00251
00252 def clearAngle(self):
00253 self.angleLock.acquire()
00254 self.__angle = 0
00255 self.angleLock.release()
00256
00257 def clear(self):
00258 self.clearDistance()
00259 self.clearAngle()
00260
00261 def __del__(self):
00262 self.port.close()
00263
00264 def brake(self):
00265 """Stops the iCreate, takes no parameters"""
00266 self.tank(0,0)
00267
00268 def demo(self, num):
00269 """Takes a single parameter, the number of the demo to begin. "Running" demo -1 will stop the demo. After using this function the iCreate must be reset."""
00270 if (num == -1):
00271 num = 255
00272
00273 self.brake()
00274 self.send(136,num)
00275 self.__sendAll()
00276
00277 def leds(self,play,advance,color,intensity):
00278 """Controls the LEDs. Parameters are play: boolean (play on/off), advance: boolean (advance on/off), color: 0-255 (how much red in the power light), and intensity: 0-255 (how bright should the power light be."""
00279 if (play):
00280 play = 1
00281 else:
00282 play = 0
00283 if (advance):
00284 advance = 1
00285 else:
00286 advance = 0
00287 bits = play << 1 | advance << 3
00288 self.send(139,bits,color,intensity)
00289
00290 def storeSong(self, num, *song):
00291 """Store a song. First parameter is the song number, the remaming arguments are taken to be of the form: note, duration, note, duration, etc. See page 12 of the iRobot Create Open Interface Manual for numerical note definitions. Duration is interpreted as duration*1/64th of a second."""
00292 if (len(song) > 32):
00293 song = song[:31]
00294 self.send(140,num,len(song)/2,*song)
00295
00296 def playSong(self,num):
00297 """Plays a song. Takes one parameter, the song number."""
00298 if (not self.songPlaying):
00299 self.send(141,num)
00300
00301 def tank(self,left,right):
00302 """Drive the iCreate like a tank (i.e. left throttle, right throttle). Takes two parameters: left and right throttle. Each can be between -500 and 500 representing mm/s. If either are outside of this range, both throttles will be linearly scaled to fit inside the range"""
00303 if abs(left) > 500 or abs(right) > 500:
00304 maxThrottle = max(abs(left),abs(right))
00305 left = 500*left/maxThrottle
00306 right = 500*right/maxThrottle
00307 lh,ll = self.__convert(left)
00308 rh,rl = self.__convert(right)
00309 self.send(145,rh,rl,lh,ll)
00310
00311 def forwardTurn(self,speed,radius):
00312 """Takes two parameters: speed and radius. Drives the iCreate at speed with enough of an angle that the iCreate will carve a circle with the given radius. Speed is in mm/s and can vary between -500 and 500. The radius can vary between -2000 and 2000 mm (with negative mm turning left)."""
00313 if (speed > 500):
00314 speed = 500
00315 if (speed < -500):
00316 speed = -500
00317 if (not (radius == 0x8000
00318 or radius == 0x7FF
00319 or radius == 0xFFFF
00320 or radius == 0x001)
00321 and
00322 (radius > 2000 or radius < -2000)):
00323 if (radius > 2000):
00324 radius = 2000
00325 if (radius < -2000):
00326 radius = -2000
00327
00328 vh,vl = self.__convert(speed)
00329 rh,rl = self.__convert(radius)
00330 self.send(137,vh,vl,rh,rl)
00331
00332 def turn(self,speed):
00333 self.right(speed)
00334
00335 def right(self,speed):
00336 """Takes in a parameter: speed and turns clockwise in place at speed mm/s."""
00337 self.forwardTurn(speed,0xFFFF)
00338
00339 def left(self,speed):
00340 """Takes in a parameter: speed and turns counter-clockwise in place at speed mm/s."""
00341 self.forwardTurn(speed,0x0001)