script-dynamixel_config

25 """ 26 SOME USEFUL DESCRIPTION 27 """ 28

29 - def __init__( self, argv, cur_nid=0x01, cfg=None ):

30 """ 31 Initialize the dynamixelConfigurator, the configurator reads in 32 arguments and sets servo paramters accordingly 33 """ 34 self.DEBUG = [] 35 self.argv = argv 36 self.cur_nid = cur_nid 37 self.new_nid = None 38 self.cfg = cfg 39 self.p = None 40 self.withReset = False 41 self.baud = None

42

43 - def usage( self ):

44 print """ 45 Usage: 46 -h, --help : Returns usage 47 -s, --scan : Scan to find all servos and baudrates on the bus 48 -c, --cur : Desired servo ID to configure 49 -n, --new : New servo ID 50 -f, --yaml : .yml file to read parameters from 51 -Q, --opaque : configure from an opaque configuration string 52 -R, --reset : Start by sending a reset command 53 -d, --debug : Set debug flag 54 -p, --port : set an alternative port for the bus 55 (see port2port.newConnection) 56 """ 57 return

58

59 - def parseArgs( self, argv ):

60 """ 61 Parse input arguments, handle if sensible 62 """ 63 if len(argv) < 1: 64 self.usage() 65 progress('No arguments passed, see Usage \n') 66 return False 67 68 try: 69 opts, args = getopt( argv, "Rhb:n:c:f:d:sp:Q:", ["help", "new:", "cur:", "yam:l", "debug", "scan", "baud:", "port:","reset","opaque:"]) 70 71 except GetoptError: 72 progress( "Invalid Arguments" ) 73 self.usage() 74 return False 75 76 scan = False 77 self.opaque = False 78 for opt, arg in opts: 79 if opt in ("-h", "--help"): 80 self.usage() 81 sys_exit() 82 elif opt in ( "-n", "--new"): 83 self.new_nid = int( arg, 0 ) 84 elif opt in ("-c", "--cur"): 85 self.cur_nid = int( arg, 0 ) 86 elif opt in ("-b", "--baud"): 87 self.baud = int(arg) 88 progress("No scan, fixed baudrate: %d " % self.baud) 89 elif opt in ("-f", "--yaml"): 90 progress("Loading yaml configuration: %s" % arg ) 91 self.cfg = load( open( arg )) 92 elif opt in ("-Q", "--opaque"): 93 progress("Configuring from opaque string: %s" % repr(arg)) 94 self.cfg = load( decodestring( arg.replace("+","\n") )) 95 self.opaque = True 96 elif opt in ("-s", "--scan"): 97 scan = True 98 elif opt in ("-R", "--reset"): 99 self.withReset = True 100 elif opt in ("-p", "--port"): 101 self.p = dynamixel.Protocol(dynamixel.Bus(port=arg)) 102 return scan

103

104 - def _defaultPort( self ):

105 self.p = dynamixel.Protocol()

106

107 - def scanAll( self ):

108 """ 109 A utility function that will scan all baudrates and return all 110 nodes found. 111 Exits upon completion 112 """ 113 if self.p is None: 114 self._defaultPort() 115 nid_str = [] 116 for baud in self._baudPlan(): 117 progress("Testing baudrate %d" % baud) 118 self.p.bus.reconnect(baudrate = baud) 119 self.p.reset() 120 for nid in self.p.pnas.keys(): 121 tpe = self.read_type(nid) 122 nid_str.append(' ID 0x%02x: %s Module found at baudrate %d ' % (nid, tpe, baud)) 123 if not nid_str: 124 progress('--- REPORT -- No modules found' ) 125 return 126 progress( '--- REPORT %s' % ('-'*50)) 127 for s in nid_str: 128 progress(s)

129

130 - def _safe_nid( self, nid):

131 """ 132 Check to ensure that we don't try to overwrite another nid 133 """ 134 if self.p.pnas.has_key(nid): 135 progress("ID 0x%02x already exists cannot set new node id" % nid) 136 return False 137 return True

138

139 - def write_nid(self, nid_old, nid_new):

140 """ 141 Write old node id to new node id 142 """ 143 if not self._safe_nid( nid_new ): 144 return 145 # Update nid if allowed 146 if nid_new == 0x01: 147 progress("Warning: NID 0x01 is reserved for configuration") 148 node = self.p.pnas[nid_old] 149 progress("Setting old NID 0x%02x to new NID 0x%02x" % (nid_old, nid_new)) 150 node.mem_write_sync( getattr( node.mm, 'ID'), nid_new) 151 return

152

153 - def write_config( self, nid, opaque = False ):

154 """ 155 Write node parameters as defined by config (cfg) 156 157 INPUT: 158 nid -- int -- node ID 159 opaque -- bool -- if true, configuration is not shown on stdout 160 """ 161 if self.cfg is None: 162 progress("No configuration defined") 163 return 164 if self.p is None: 165 self._defaultPort() 166 baud = None 167 # For memory address names set those attributes 168 node = self.p.pnas[nid] 169 if self.opaque: 170 progress('Configuring ID 0x%02x with parameters %s' % (nid, self.cfg.keys())) 171 else: 172 progress('Configuring ID 0x%02x with parameters %s' % (nid, self.cfg)) 173 node = self.p.pnas[nid] 174 for name, val in self.cfg.iteritems(): 175 # Check for node id setting 176 if not opaque: 177 print name, val 178 if name == 'ID': 179 print " Setting ID through .yml file not allowed, see --help" 180 sys_exit(-7) 181 # Check for baudrate change, if so, write baud last 182 if name == 'baud': 183 baud = val 184 continue 185 # This is pulled out of DynamixelModule 186 if hasattr( node.mm, name ): 187 if opaque: 188 print "Writing %s" % repr(name) 189 else: 190 print "Writing %s to %s" % (repr(name), repr(val)) 191 node.mem_write_sync( getattr( node.mm, name ), val ) 192 else: 193 raise KeyError("Unknown address '%s'" % name) 194 # Write baudrate last 195 if baud is not None: 196 print "Writing baud rate ",baud 197 node.mem_write_sync( getattr( node.mm, 'baud' ), baud )

198

199 - def read_type(self, nid):

200 """ 201 Read the servo type, returned upon scanAll, ... maybe scan? 202 """ 203 if self.p is None: 204 self._defaultPort() 205 node = self.p.pnas[nid] 206 cw_limit = node.mem_read_sync( node.mm.cw_angle_limit) 207 ccw_limit = node.mem_read_sync( node.mm.ccw_angle_limit) 208 if cw_limit==0 and ccw_limit==0: 209 return 'CR' 210 return 'Servo'

211

212 - def _baudPlan( self ):

213 """(private) plan order in which baudrates are scanned 214 """ 215 if self.baud: 216 return [self.baud] 217 bauds = [ b 218 for b in self.p.bus.getSupportedBaudrates() 219 if (b>=1200 and b<=1e6 and not b in [57600,115200]) ] 220 bauds.sort(reverse=True) 221 bauds.insert(0, 57600) 222 bauds.insert(0, 115200) 223 return bauds

224

225 - def scan(self, nid, timeout=0.5, retries=1 ):

226 """ 227 Check if a given node id exists on any baudrates, 228 returns nid if successful, None if not 229 230 INPUTS: 231 p -- object -- protocol object 232 nid -- int -- node id to scan for 233 234 """ 235 if self.p is None: 236 self._defaultPort() 237 progress("Scanning for 0x%02X" % nid) 238 for baud in self._baudPlan(): 239 self.p.bus.reconnect(baudrate = baud) 240 while True: 241 try: 242 self.p.reset(nodes=[nid], ping_rate=0.1) 243 break 244 except dynamixel.ProtocolError,pe: 245 print str(pe), "retrying..." 246 if self.p.pnas.has_key(nid): 247 tpe = self.read_type(nid) 248 progress(' ID 0x%02x: %s Module found at baudrate %d ' % (nid, tpe, baud)) 249 return 250 progress( 'ID 0x%02x not found' % nid ) 251 return

252

253 - def reset_nid( self ):

254 """ 255 Reset servo at cur_nid 256 """ 257 if not self._safe_nid(1): 258 progress("A node ID 0x01 already exists on the bus. Reset aborted") 259 return 260 progress("Resetting 0x%02X... will be to 0x01, at 57600 baud") 261 self.p.bus.reset_nid( self.cur_nid ) 262 self.p.bus.reconnect( 57600 ) 263 self.cur_nid = 1

264

265 - def run( self ):

266 """ 267 Read in and parse arguments, perform necessary configuration 268 """ 269 scan = self.parseArgs( self.argv ) 270 if scan: 271 return self.scanAll() 272 # Find desired nid 273 self.scan( self.cur_nid ) 274 if self.withReset: 275 self.reset_nid() 276 # Check that new nid is safe 277 if not self._safe_nid( self.new_nid ): 278 return 279 # Configure, if requested 280 if self.cfg: 281 self.write_config( self.cur_nid, self.opaque ) 282 # After changing configuration we may be at a new baudrate so rescan 283 if self.cfg.has_key('baud') and (self.new_nid != self.cur_nid): 284 self.scan( self.cur_nid ) 285 # If nid change requested --> do it 286 if self.new_nid: 287 self.write_nid( self.cur_nid, self.new_nid ) 288 return

Source Code for Script script-dynamixel_config_py