create.py
Go to the documentation of this file.
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() #send queued commands
00080 
00081                         self.create.send(149,len(self.packets),*[i[0] for i in self.packets]) #read sensor 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 #atomic
00112                                 else:
00113                                         self.packetRef.append(data) #atomic
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) #emergency brake
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                 #should have processed initialization
00177                 self.port.flushInput() #ignore rest of input
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 #               self.stop()
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)


irobot_create_2_1
Author(s): Graylin Trevor Jay, Brian Thomas
autogenerated on Fri Jan 3 2014 11:10:15