Source Code for Package barobo

  1  #!/usr/bin/env python 
  2   
  3  """ 
  4  The Barobo Python Module 
  5   
  6  This python module can be used to control Barobo robots. The easiest way to use 
  7  this package is in conjunction with BaroboLink. After connecting to the robots 
  8  you want to control in BaroboLink, the following python program will move 
  9  joints 1 and 3 on the first connected Linkbot in BaroboLink:: 
 10   
 11      from barobo import Linkbot 
 12      linkbot = Linkbot() 
 13      linkbot.connect() 
 14      linkbot.moveTo(180, 0, -180) 
 15   
 16  You may also use this package to control Linkbots without BaroboLink. In that 
 17  case, a typical control program will look something like this:: 
 18      from barobo import Linkbot, Dongle 
 19   
 20      dongle = Dongle() 
 21      dongle.connect() # Connect to the dongle 
 22      linkbot = dongle.getLinkbot() # or linkbot = dongle.getLinkbot('2B2C') where  
 23                                    # '2B2C' should be replaced with the serial ID  
 24                                    # of your Linkbot. Note that the serial ID  
 25                                    # used here can be that of a nearby Linkbot  
 26                                    # that you wish to connect to wirelessly. If  
 27                                    # no serial ID is provided, the new linkbot  
 28                                    # will refer to the Linkbot currently  
 29                                    # connected via USB. 
 30                                    # Also, note that this function can be called 
 31                                    # multiple times to retrieve handles to  
 32                                    # multiple wireless Linkbots, which can all 
 33                                    # be controlled in the same Python script. 
 34      linkbot.moveTo(180, 0, -180)  # Move joint 1 180 degrees in the positive  
 35                                    # direction, joint 3 180 degrees in the  
 36                                    # negative direction 
 37   
 38  For more documentation, please refer to the documentation under the 
 39  L{Linkbot<barobo.linkbot.Linkbot>} class. 
 40  """ 
 41   
 42  import struct 
 43  import sys 
 44  try: 
 45      import Queue 
 46  except: 
 47      import queue as Queue 
 48  import threading 
 49  import barobo._comms as _comms 
 50  from barobo.linkbot import Linkbot 
 51  from barobo.mobot import Mobot 
 52   
 53  ROBOTFORM_MOBOT=1 
 54  ROBOTFORM_I=2 
 55  ROBOTFORM_L=3 
 56  ROBOTFORM_T=4 
 57   
 58  ROBOT_NEUTRAL=0 
 59  ROBOT_FORWARD=1 
 60  ROBOT_BACKWARD=2 
 61  ROBOT_HOLD=3 
 62  ROBOT_POSITIVE=4 
 63  ROBOT_NEGATIVE=5 
 64   
 65  PINMODE_INPUT = 0x00 
 66  PINMODE_OUTPUT = 0x01 
 67  PINMODE_INPUTPULLUP = 0x02 
 68   
 69  AREF_DEFAULT = 0x00 
 70  AREF_INTERNAL = 0x01 
 71  AREF_INTERNAL1V1 = 0x02 
 72  AREF_INTERNAL2V56 = 0x03 
 73  AREF_EXTERNAL = 0x04 
 74   
 75  import os 
 76  if os.name == 'nt': 
 77      if sys.version_info[0] == 3: 
 78          import winreg 
 79      else: 
 80          import _winreg as winreg 
 81   
 82   
 83  if sys.platform.startswith('linux'): 
 84 -    def __FROM(x): 
 85          return ' find ' + x + ' -maxdepth 0 -print ' 
 86   
 87 -    def __SELECT(): 
 88          return " | xargs -I}{ find '}{' " 
 89   
 90 -    def __AND(): 
 91          return __SELECT() + ' -maxdepth 1 ' 
 92   
 93 -    def __SUBSYSTEM(x): 
 94          return " -type l -name subsystem -lname \\*/" + x + " -printf '%h\\n' " 
 95   
 96 -    def __SUBSYSTEMF(x): 
 97          return " -type l -name subsystem -lname \\*/" +x+ " -printf '%%h\\n' " 
 98   
 99 -    def __SYSATTR(x, y): 
100          return " -type f -name " +x+ " -execdir grep -q '" + \ 
101            y+ "' '{}' \\; -printf '%h\\n' " 
102   
103 -    def __SYSATTRF(x, y): 
104          return " -type f -name " +x+ " -execdir grep -q '" + \ 
105            y+ "' '{}' \\; -printf '%%h\\n' " 
106   
107 -    def __FIRST(): 
108          return " -quit " 
109   
110 -    def __SELECTUP(): 
111          return " | xargs -I}{ sh -c 'x=\"}{\"; while [ \"/\" != \"$x\" ]; " \ 
112                 "do dirname \"$x\"; x=$(dirname \"$x\"); done' " + __AND() 
113   
114 -    def findDongle(): 
115          dongleIDs = [ ('Barobo, Inc.', 'Mobot USB-Serial Adapter'), 
116                        ('Barobo, Inc.', 'Linkbot USB-Serial Adapter'), 
117                        ('Barobo, Inc.', 'Barobo USB-Serial Adapter') ] 
118          import os 
119          import subprocess 
120          try:  
121              sysfs = os.environ['SYSFS_PATH'] 
122          except: 
123              sysfs = '/sys' 
124          for (manufacturer, productid) in dongleIDs: 
125              cmd = __FROM(sysfs+'/devices')+__SELECT()+\ 
126                    __SYSATTR('manufacturer', manufacturer)+\ 
127                    __AND() + __SYSATTR('product', productid) +\ 
128                    __SELECT() + __SUBSYSTEM('tty') +\ 
129                    " | xargs -I{} grep DEVNAME '{}'/uevent"    +\ 
130                    " | cut -d= -f2" 
131              p = subprocess.check_output(['/bin/sh', '-c', cmd]) 
132              if len(p) > 1: 
133                  return (str('/dev/')+p.decode('utf-8')).rstrip() 
134   
135 -def _getSerialPorts(): 
136      import serial 
137      if os.name == 'nt': 
138          available = [] 
139          handle = winreg.OpenKey(winreg.HKEY_LOCAL_MACHINE,  
140              'HARDWARE\\DEVICEMAP\\SERIALCOMM') 
141          for i in range(256): 
142              try: 
143                  name, port, _ = winreg.EnumValue(handle, i) 
144                  if name[:14] == '\\Device\\USBSER': 
145                      available.append(port) 
146              except: 
147                  break 
148          return available 
149      else: 
150          from serial.tools import list_ports 
151          return [port[0] for port in list_ports.comports()] 
152   
153  # Check if a device connected at 'comport' is a Linkbot 
154 -def __checkLinkbotTTY(comport): 
155      import serial 
156      s = serial.Serial(comport, baudrate=230400) 
157      s.timeout = 2 
158      numtries = 0 
159      maxtries = 3 
160      while numtries < maxtries: 
161          try: 
162              s.write(bytearray([0x30, 0x03, 0x00])) 
163              r = s.recv(3) 
164              if r == [0x10, 0x03, 0x11]: 
165                  break 
166          except: 
167              if numtries < maxtries: 
168                  numtries+=1 
169              else: 
170                  return True 
171   
172 -def _unpack(fmt, buffer): 
173      if sys.version_info[0] == 2 and sys.version_info[1] == 6: 
174          return struct.unpack(fmt, bytes(buffer)) 
175      elif sys.version_info[0] == 3: 
176          return struct.unpack(fmt, buffer) 
177      else: 
178          return struct.unpack(fmt, str(buffer)) 
179   
180   
181 -class BaroboException(Exception): 
182 -    def __init__(self, *args, **kwargs): 
183          Exception.__init__(self, *args, **kwargs) 
184   
185 -class Dongle(): 
186      """ 
187      The BaroboCtx (BaroboContext) is the entity which manages all of the  
188      Linkbots in a computational environment. If loosely represents a ZigBee  
189      dongle which can communicate and with and control all Linkbots within its  
190      communication range.  
191      """ 
192      RESP_OK = 0x10 
193      RESP_END = 0x11 
194      RESP_ERR = 0xff 
195      RESP_ALREADY_PAIRED = 0xfe 
196   
197      EVENT_BUTTON = 0x20 
198      EVENT_REPORTADDRESS = 0x21 
199      TWI_REGACCESS = 0x22 
200      EVENT_DEBUG_MSG = 0x23 
201      EVENT_JOINT_MOVED = 0x24 
202      EVENT_ACCEL_CHANGED = 0x25 
203   
204   
205      CMD_STATUS = 0x30 
206      CMD_DEMO = 0x31 
207      CMD_SETMOTORDIR = 0x32 
208      CMD_GETMOTORDIR = 0x33 
209      CMD_SETMOTORSPEED = 0x34 
210      CMD_GETMOTORSPEED = 0x35 
211      CMD_SETMOTORANGLES = 0x36 
212      CMD_SETMOTORANGLESABS = 0x37 
213      CMD_SETMOTORANGLESDIRECT = 0x38 
214      CMD_SETMOTORANGLESPID = 0x39 
215      CMD_GETMOTORANGLES = 0x3A 
216      CMD_GETMOTORANGLESABS = 0x3B 
217      CMD_GETMOTORANGLESTIMESTAMP = 0x3C 
218      CMD_GETMOTORANGLESTIMESTAMPABS = 0x3D 
219      CMD_SETMOTORANGLE = 0x3E 
220      CMD_SETMOTORANGLEABS = 0x3F 
221      CMD_SETMOTORANGLEDIRECT = 0x40 
222      CMD_SETMOTORANGLEPID = 0x41 
223      CMD_GETMOTORANGLE = 0x42 
224      CMD_GETMOTORANGLEABS = 0x43 
225      CMD_GETMOTORANGLETIMESTAMP = 0x44 
226      CMD_GETMOTORSTATE = 0x45 
227      CMD_GETMOTORMAXSPEED = 0x46 
228      CMD_GETENCODERVOLTAGE = 0x47 
229      CMD_GETBUTTONVOLTAGE = 0x48 
230      CMD_GETMOTORSAFETYLIMIT = 0x49 
231      CMD_SETMOTORSAFETYLIMIT = 0x4A 
232      CMD_GETMOTORSAFETYTIMEOUT = 0x4B 
233      CMD_SETMOTORSAFETYTIMEOUT = 0x4C 
234      CMD_STOP = 0x4D 
235      CMD_GETVERSION = 0x4E 
236      CMD_BLINKLED = 0x4F 
237      CMD_ENABLEBUTTONHANDLER = 0x50 
238      CMD_RESETABSCOUNTER = 0x51 
239      CMD_GETHWREV = 0x52 
240      CMD_SETHWREV = 0x53 
241      CMD_TIMEDACTION = 0x54 
242      CMD_GETBIGSTATE = 0x55 
243      CMD_SETFOURIERCOEFS = 0x56 
244      CMD_STARTFOURIER = 0x57 
245      CMD_LOADMELODY = 0x58 
246      CMD_PLAYMELODY = 0x59 
247      CMD_GETADDRESS = 0x5A 
248      CMD_QUERYADDRESSES = 0x5B 
249      CMD_GETQUERIEDADDRESSES = 0x5C 
250      CMD_CLEARQUERIEDADDRESSES = 0x5D 
251      CMD_REQUESTADDRESS = 0x5E 
252      CMD_REPORTADDRESS = 0x5F 
253      CMD_REBOOT = 0x60 
254      CMD_GETSERIALID = 0x61 
255      CMD_SETSERIALID = 0x62 
256      CMD_SETRFCHANNEL = 0x63 
257      CMD_FINDMOBOT = 0x64 
258      CMD_PAIRPARENT = 0x65 
259      CMD_UNPAIRPARENT = 0x66 
260      CMD_RGBLED = 0x67 
261      CMD_SETMOTORPOWER = 0x68 
262      CMD_GETBATTERYVOLTAGE = 0x69 
263      CMD_BUZZERFREQ = 0x6A 
264      CMD_GETACCEL = 0x6B 
265      CMD_GETFORMFACTOR = 0x6C 
266      CMD_GETRGB = 0x6D 
267      CMD_GETVERSIONS = 0x6E 
268      CMD_PLACEHOLDER201304121823 = 0x6F 
269      CMD_PLACEHOLDER201304152311 = 0x70 
270      CMD_PLACEHOLDER201304161605 = 0x71 
271      CMD_PLACEHOLDER201304181705 = 0x72 
272      CMD_PLACEHOLDER201304181425 = 0x73 
273      CMD_SET_GRP_MASTER = 0x74 
274      CMD_SET_GRP_SLAVE = 0x75 
275      CMD_SET_GRP = 0x76 
276      CMD_SAVE_POSE = 0x77 
277      CMD_MOVE_TO_POSE = 0x78 
278      CMD_IS_MOVING = 0x79 
279      CMD_GET_MOTOR_ERRORS = 0x7A 
280      CMD_MOVE_MOTORS = 0x7B 
281      CMD_TWI_SEND = 0x7C 
282      CMD_TWI_RECV = 0x7D 
283      CMD_TWI_SENDRECV = 0x7E 
284      CMD_SET_ACCEL = 0x7F 
285      CMD_SMOOTHMOVE = 0x80 
286      CMD_SETMOTORSTATES = 0x81 
287      CMD_SETGLOBALACCEL = 0x82 
288      CMD_PING = 0x89 
289      CMD_GET_HW_REV = 0x8A 
290      CMD_SET_HW_REV = 0x8B 
291      CMD_SET_JOINT_EVENT_THRESHOLD = 0x8C 
292      CMD_SET_ENABLE_JOINT_EVENT = 0x8D 
293      CMD_SET_ACCEL_EVENT_THRESHOLD = 0x8E 
294      CMD_SET_ENABLE_ACCEL_EVENT = 0x8F 
295   
296      MOTOR_FORWARD = 1 
297      MOTOR_BACKWARD = 2 
298   
299      TWIMSG_HEADER = 0x22 
300      TWIMSG_REGACCESS = 0x01 
301      TWIMSG_SETPINMODE = 0x02 
302      TWIMSG_DIGITALWRITEPIN = 0x03 
303      TWIMSG_DIGITALREADPIN = 0x04 
304      TWIMSG_ANALOGWRITEPIN = 0x05 
305      TWIMSG_ANALOGREADPIN = 0x06 
306      TWIMSG_ANALOGREF = 0x07 
307   
308 -    def __init__(self): 
309          # Queue going to the robot 
310          self.writeQueue = Queue.Queue()  
311          # Queue coming from the robot 
312          self.readQueue = Queue.Queue() 
313          # Queue coming from the robot intended for the content 
314          self.ctxReadQueue = Queue.Queue() 
315          self.link = None 
316          self.phys = None 
317          self.children = {} # List of Linkbots 
318          self.scannedIDs = {} 
319          self.scannedIDs_cond = threading.Condition() 
320          self.giant_lock = threading.Lock() 
321          pass 
322   
323 -    def __init_comms(self): 
324          self.commsInThread = threading.Thread(target=self._commsInEngine) 
325          self.commsInThread.daemon = True 
326          self.commsInThread.start() 
327          self.commsOutThread = threading.Thread(target=self._commsOutEngine) 
328          self.commsOutThread.daemon = True 
329          self.commsOutThread.start() 
330   
331 -    def addLinkbot(self, linkbot): 
332          self.children[linkbot.getSerialID()] = linkbot 
333   
334 -    def autoConnect(self): 
335          try: 
336              self.connectBaroboBrowser() 
337          except: 
338              if os.name == 'nt': 
339                  myports = _getSerialPorts() 
340              else: 
341                  myports = [findDongle()] 
342              connected = False 
343              for port in myports: 
344                  try: 
345                      self.connectDongleSFP(port) 
346                      connected = True 
347                  except: 
348                      pass 
349              if not connected: 
350                  raise BaroboException('Could not find attached dongle.') 
351   
352 -    def connect(self): 
353          """ 
354          Automatically connect to an attached Barobo Dongle. Throw an  
355          exception if no dongle is found. 
356          """ 
357          self.autoConnect() 
358   
359 -    def connectBaroboBrowser(self): 
360          """ 
361          Connect the dongle to BaroboBrowser 
362          """ 
363          self.phys = _comms.PhysicalLayer_Socket('localhost', 5769) 
364          self.link = _comms.LinkLayer_TTY(self.phys, self.handlePacket) 
365          self.link.start() 
366          self.__init_comms() 
367          try: 
368              self.__init_comms() 
369              self.__checkStatus() 
370              self.__getDongleID() 
371          except Exception as e: 
372              self.phys.close() 
373              self.link.stop() 
374              raise e 
375   
376 -    def connectBaroboLink(self): 
377          """ 
378          Connect the BaroboContext to BaroboLink. 
379          """ 
380          # Try to connect to BaroboLink running on localhost 
381          self.phys = _comms.PhysicalLayer_Socket('localhost', 5768) 
382          self.link = _comms.LinkLayer_Socket(self.phys, self.handlePacket) 
383          self.link.start() 
384          self.__init_comms() 
385          self.__getDongleID() 
386   
387 -    def connectBluetooth(self, macaddr): 
388          """ 
389          Connect the BaroboContext to a Bluetooth LinkPod. 
390          """ 
391          self.phys = _comms.PhysicalLayer_Bluetooth(macaddr) 
392          #self.link = _comms.LinkLayer_Socket(self.phys, self.handlePacket) 
393          self.link = _comms.LinkLayer_TTY(self.phys, self.handlePacket) 
394          self.link.start() 
395          try: 
396              self.__init_comms() 
397              self.__checkStatus() 
398              self.__getDongleID() 
399          except: 
400              raise BaroboException( 
401                  'Could not connect to Bluetooth at {0}'.format(macaddr)) 
402   
403 -    def connectMobotBluetooth(self, macaddr): 
404          """ 
405          Connect the BaroboContext to a Bluetooth Mobot or legacy Bluetooth  
406          Linkbot. 
407          """ 
408          self.phys = _comms.PhysicalLayer_Bluetooth(macaddr) 
409          self.link = _comms.LinkLayer_Socket(self.phys, self.handlePacket) 
410          self.link.start() 
411          try: 
412              self.__init_comms() 
413          except: 
414              raise BaroboException( 
415                  'Could not connect to Bluetooth at {0}'.format(macaddr)) 
416   
417 -    def connectDongleTTY(self, ttyfilename): 
418          """ 
419          Connect the BaroboCtx to a Linkbot that is connected with a USB cable. 
420          """ 
421          self.phys = _comms.PhysicalLayer_TTY(ttyfilename) 
422          self.link = _comms.LinkLayer_TTY(self.phys, self.handlePacket) 
423          self.link.start() 
424          try: 
425              self.__init_comms() 
426              self.__checkStatus() 
427              self.__getDongleID() 
428          except: 
429              self.phys.close() 
430              self.link.stop() 
431              self.connectDongleSFP(ttyfilename) 
432   
433 -    def connectDongleSFP(self, ttyfilename): 
434          """ 
435          Connect the BaroboCtx to a Linkbot using libsfp that is connected with a  
436          USB cable. 
437          """ 
438          self.phys = _comms.PhysicalLayer_TTY(ttyfilename) 
439          self.link = _comms.LinkLayer_SFP(self.phys, self.handlePacket) 
440          self.link.start() 
441          try: 
442              self.__init_comms() 
443              self.__checkStatus() 
444              self.__getDongleID() 
445          except: 
446              raise BaroboException( 
447                  'Could not connect to dongle at {0}'.format(ttyfilename)) 
448   
449 -    def disconnect(self): 
450          self.link.stop() 
451          self.phys.disconnect() 
452          self.children = {} 
453   
454 -    def handlePacket(self, packet): 
455          self.readQueue.put(packet) 
456   
457 -    def scanForRobots(self): 
458          buf = [ self.CMD_QUERYADDRESSES, 3, 0x00 ] 
459          self.writePacket(_comms.Packet(buf, 0x0000)) 
460   
461 -    def getScannedRobots(self): 
462          return self.scannedIDs 
463   
464 -    def getLinkbot(self, serialID=None, linkbotClass=None): 
465          if serialID is None: 
466              self.giant_lock.acquire() 
467              serialID = list(self.scannedIDs.keys())[0] 
468              self.giant_lock.release() 
469           
470          serialID = serialID.upper() 
471          if serialID not in self.scannedIDs: 
472              self.findRobot(serialID) 
473              self.waitForRobot(serialID) 
474          if linkbotClass is None: 
475              linkbotClass = Linkbot 
476          if serialID in self.children: 
477              return self.children[serialID] 
478          l = linkbotClass() 
479          l.zigbeeAddr = self.scannedIDs[serialID] 
480          l.serialID = serialID 
481          l.baroboCtx = self 
482          self.children[serialID] = l 
483          l.form = l.getFormFactor() 
484          if l.zigbeeAddr != self.zigbeeAddr: 
485              l._pairParent() 
486          return l 
487   
488 -    def findRobot(self, serialID): 
489          if serialID in self.scannedIDs: 
490              return 
491          buf = bytearray([ self.CMD_FINDMOBOT, 7 ]) 
492          buf += bytearray(serialID.encode('ascii')) 
493          buf += bytearray([0]) 
494          self.writePacket(_comms.Packet(buf, 0x0000)) 
495   
496 -    def waitForRobot(self, serialID, timeout=2.0): 
497          self.scannedIDs_cond.acquire() 
498          numtries = 0 
499          while serialID not in self.scannedIDs: 
500              self.scannedIDs_cond.wait(2) 
501              numtries += 1 
502              if numtries >= 3: 
503                  self.scannedIDs_cond.release() 
504                  raise BaroboException('Robot {0} not found.'.format(serialID)) 
505          self.scannedIDs_cond.release() 
506          return serialID in self.scannedIDs 
507   
508 -    def writePacket(self, packet): 
509          self.writeQueue.put(packet) 
510   
511 -    def _commsInEngine(self): 
512          while True: 
513              packet = self.readQueue.get(block=True, timeout=None) 
514              # First, see if these are "Report Address" events. We want to filter 
515              # those out and use them for our own purposes 
516              if packet.data[0] == self.EVENT_REPORTADDRESS: 
517                  botid = _unpack('!4s', packet.data[4:8])[0] 
518                  zigbeeAddr = _unpack('!H', packet[2:4])[0] 
519                  if botid not in self.scannedIDs: 
520                      self.scannedIDs_cond.acquire() 
521                      self.scannedIDs[botid.decode('ascii')] = zigbeeAddr 
522                      self.scannedIDs_cond.notify() 
523                      self.scannedIDs_cond.release() 
524                  continue 
525              elif packet.data[0] == self.EVENT_DEBUG_MSG: 
526                  print (packet.data[2:]) 
527                  continue 
528              # Get the zigbee address from the packet  
529              zigbeeAddr = packet.addr 
530              if 0 == zigbeeAddr: 
531                  self.ctxReadQueue.put(packet) 
532                  continue 
533              for _, linkbot in self.children.items(): 
534                  if zigbeeAddr == linkbot.zigbeeAddr: 
535                      linkbot.readQueue.put(packet, block=True) 
536                      break 
537   
538 -    def _commsOutEngine(self): 
539          while True: 
540              packet = self.writeQueue.get() 
541              self.link.write(packet.data, packet.addr) 
542   
543 -    def __checkStatus(self): 
544          numtries = 0 
545          maxtries = 3 
546          while True: 
547              buf = [ self.CMD_STATUS, 3, 0x00 ] 
548              self.writePacket(_comms.Packet(buf, 0x0000)) 
549              try: 
550                  response = self.ctxReadQueue.get(block=True, timeout=2.0) 
551                  break 
552              except: 
553                  if numtries < maxtries: 
554                      numtries+=1 
555                      continue 
556                  else: 
557                      raise 
558   
559 -    def __getDongleID(self): 
560          numtries = 0 
561          maxtries = 3 
562          while True: 
563              buf = [ self.CMD_GETSERIALID, 3, 0x00 ] 
564              self.writePacket(_comms.Packet(buf, 0x0000)) 
565              try: 
566                  response = self.ctxReadQueue.get(block=True, timeout=2.0) 
567                  break 
568              except: 
569                  if numtries < maxtries: 
570                      numtries+=1 
571                      continue 
572                  else: 
573                      raise 
574          serialID = _unpack('!4s', response[2:6])[0].decode('UTF-8') 
575          buf = [self.CMD_GETADDRESS, 3, 0x00] 
576          self.writePacket(_comms.Packet(buf, 0x0000)) 
577          response = self.ctxReadQueue.get(block=True, timeout=2.0) 
578          zigbeeAddr = _unpack('!H', response[2:4])[0] 
579          self.zigbeeAddr = zigbeeAddr 
580          self.scannedIDs[serialID] = zigbeeAddr 
581       
582  BaroboCtx = Dongle 
583