Source Code for Module botscan

  1  import sys 
  2  import yaml 
  3  from joy import * 
  4  import ckbot.hitec as hitec 
  5  import ckbot.pololu as pololu 
  6  import ckbot.dynamixel as dynamixel 
  7   
  8 -class ScanRobot( JoyApp ): 
  9 -  def __init__(self,robot, cfg={}): 
 10      cfg0 = dict(duration=30, rate=1) 
 11      cfg0.update(cfg) 
 12      JoyApp.__init__( self, robot=robot, cfg=cfg0 ) 
 13      self.robotPop = robot 
 14       
 15 -  def onStart( self ): 
 16      self.next = self.now + self.cfg.rate 
 17      self.stopTime = self.now + self.cfg.duration 
 18       
 19 -  def onEvent( self, evt ): 
 20      if evt.type==KEYDOWN and evt.key in [32,13]: 
 21        self.robot.populate( **self.robotPop ) 
 22        return 
 23      if evt.type==QUIT or evt.type==KEYDOWN: 
 24        self.stop() 
 25      if self.now < self.next:  
 26        return 
 27      lst = [] 
 28      for nid in self.robot.getLive(): 
 29        mod = self.robot.get(nid,None) 
 30        if mod is None: 
 31          lst.append('<Unknown 0x%02x>' % nid) 
 32        else: 
 33          if hasattr(mod,'go_slack'): 
 34            mod.go_slack() 
 35          lst.append(mod.name) 
 36      progress( "Seeing: %s" % ", ".join(lst) ) 
 37      self.next = self.now + self.cfg.rate 
 38      if self.now>self.stopTime: 
 39        self.stop() 
 40   
 41  if __name__=="__main__": 
 42    robot = {} 
 43    cfg = {} 
 44    # Table of available busses 
 45    busTbl = { 
 46      'hitec' : hitec, 
 47      'pololu' : pololu, 
 48      'dynamixel' : dynamixel, 
 49      'h' : hitec, 
 50      'p' : pololu, 
 51      'd' : dynamixel 
 52    } 
 53    bus = None 
 54    args = list(sys.argv[1:]) 
 55    while args: 
 56      arg = args.pop(0) 
 57      if arg=='--mod-count' or arg=='-c': 
 58        N = int(args.pop(0)) 
 59        robot.update(count=N) 
 60      elif arg=='--names' or arg=='-n': 
 61        names = yaml.safe_load( '{%s}'% args.pop(0) ) 
 62        robot.update( names=names ) 
 63      elif arg=='--walk' or arg=='-w': 
 64        robot.update( walk=True ) 
 65      elif arg=='--any' or arg=='-a': 
 66        robot.update( count=None ) 
 67      elif arg=='--duration' or arg=='-d': 
 68        cfg.update( duration = float(args.pop(0)) ) 
 69      elif arg=='--port' or arg=='-p': 
 70        cfg.update( port = args.pop(0)) 
 71      elif arg=='--frequency' or arg=='-f': 
 72        cfg.update( rate = float(args.pop(0)) ) 
 73      elif arg=='--bus' or arg=='-b': 
 74        bus = busTbl.get(args.pop(0),None) 
 75        if bus is None: 
 76          sys.stderr.write("Unknown bus '%s'\n" % bus) 
 77          sys.exit(2)    
 78        robot['protocol']=bus 
 79      elif arg=='--help' or arg=='-h': 
 80        robot=None 
 81        break 
 82      else: 
 83        sys.stderr.write("Unknown option '%s'\n" % arg ) 
 84        sys.exit(1) 
 85      # ENDS cmdline parsing loop 
 86    # Usage message, if needed 
 87    if not robot: 
 88        sys.stdout.write(""" 
 89    Usage: %s [options] 
 90     
 91      Scan bus for robot 
 92       
 93      Options:       
 94        --mod-count <number> | -c <number> 
 95          Search for specified number of modules at startup 
 96         
 97        --names <names> | -n <names> 
 98          Where <names> is a valid YAML mapping from node numbers 
 99          to names, e.g. '0x36: head, 223: tail' 
100         
101        --walk | -w 
102          Attempt to walk the Object Dictionary of the modules found 
103         
104        --bus <bus-name> | -b <bus-name> 
105          Supported busses are 'hitec', 'pololu' and 'dynamixel' 
106          Bus names may be abbreviated to a single character 
107         
108        --port <device-port> | -p <device-string> 
109          Access the bus on the specified device. Device strings are 
110          described in port2port.py newConnection 
111           
112        --any | -a 
113          Scan any number of modules 
114      """ % sys.argv[0]) 
115        sys.exit(1) 
116    app = ScanRobot(robot,cfg=cfg) 
117    app.run() 
118