ckbot.pololu

1 """ 2 The CKBot.pololu python module provides classes used to communicate with CKBot robot modules connected to a Pololu Maestro Device (rather than CAN) using a protocol similar to the Robotics Bus Protocol. These classes are primarily used to interface with the CKBot.logical python module found in the Modlab CKBot repository. For more information on 3 the low-level interface, please refer to the Pololu Maestro Device User Manual found at http://www.pololu.com/docs/0J40/all 4 5 NOTE: Our implementation assumes the Pololu Maestro Device is set receive commands in Pololu Mode. 6 7 Main uses of this module: 8 (*) control CKBot robot modules (specifically the servos) connected to a Pololu Maestro device 9 (*) mimics the behaviour of the CAN Bus, except using the Pololu Maestro Device as a communications channel, can currently only 10 send position and go_slack commands 11 12 Example 1 - Pololu robot module only: 13 nodes = {0:0x23, 5:0x65} 14 bus = pololu.Bus() 15 p = pololu.Protocol(bus = bus, nodes = nodes) 16 p.send_cmd(0x23,4,1000) 17 18 Example 2 - Integrate with CKBot.logical python module: 19 import logical 20 nodes = {0xD1:0, 0xA7:1, 0xDA:2} 21 bus = pololu.Bus() #J on mac for now do .Bus("/dev/tty.usbserial-A70041hF") 22 p = pololu.Protocol(bus = bus, nodes = nodes) 23 c = logical.Cluster(p) 24 c.populate(3,{0xD1:'head', 0xA7:'mid', 0xDA:'tail'}) 25 c.at.head.set_pos(1000) 26 27 Both examples sets the position of the 0x23 robot module, connected to Pololu Maestro channel 0, to 1000 (10 degrees from the neutral position) 28 """ 29 30 from time import time as now 31 from sys import platform as SYS_PLATFORM 32 import struct 33 import commands 34 35 from ckmodule import Module, AbstractNodeAdaptor, AbstractProtocol, AbstractBus, progress, AbstractServoModule 36 from port2port import newConnection 37 38 DEBUG=[]

39 40 -class Bus(AbstractBus):

41 """ 42 Concrete class that provides the functionality 43 needed to send messages to a pololu controller 44 over a serial connection. 45 46 It is responsible for correctly formatting certain inputs 47 to Pololu-understandable formats as documented in the Pololu 48 Maestro User Manual located at http://www.pololu.com/docs/0J40/all 49 """ 50

51 - def __init__(self, port = 'tty={ "glob":"/dev/ttyACM0", "baudrate":38400, "timeout":0.1}', crc_enabled=False, *args,**kw):

52 """ 53 Initialize a Pololu Bus class 54 55 INPUT: 56 port -- port / connection specification (see port2port.Connection) 57 58 ATTRIBUTES: 59 ser -- connection handle 60 """ 61 AbstractBus.__init__(self,*args,**kw) 62 self.ser = newConnection( port ) 63 self.port = port 64 self.crc_enabled = crc_enabled 65 self.DEBUG = DEBUG

66

67 - def get_errors(self):

68 """ 69 Retrieve error messages from the Pololu Maestro device using the protocol outlined by the Pololu Maestro documentation 70 71 WARNING: This method will hang if there is a faulty serial connection 72 """ 73 if self.ser is not None: 74 self.write((0xA1,0)) 75 76 while not self.ser.inWaiting(): 77 continue 78 79 high = self.ser.read() 80 while self.ser.inWaiting(): 81 low = high 82 high = self.ser.read() 83 84 return low | (high << 8)

85

86 - def open( self ):

87 if not self.ser.isOpen(): 88 raise IOError("Serial port is not open")

89

90 - def write(self, val):

91 """ 92 Write data to the pololu controller over serial 93 94 INPUT: 95 val -- tuple -- tuple of ints to write to serial 96 """ 97 98 if self.ser is None: 99 raise IOError("Serial port is not open") 100 101 # Format the values into serial-writable string 102 packed_cmd = [ struct.pack("B", val_part) \ 103 for val_part in val ] 104 cmd_str = "".join(packed_cmd) 105 106 if self.crc_enabled: 107 cmd_str = self.crc7(cmd_str) # Calculate and append Cyclic Redundancy Check byte 108 if 'w' in self.DEBUG: 109 print "Ser WR>",repr(cmd_str) 110 self.ser.write(cmd_str)

111

112 - def close(self):

113 """ 114 Close serial connection to the pololu controller if 115 a connection has been made 116 """ 117 if self.ser is not None: 118 self.ser.close() 119 self.ser = None

120

121 - def crc7(self,comstr):

122 """ 123 This function calculates and appends the Cyclic Redundancy Check (CRC7) byte for error checking 124 """ 125 l = len(comstr) 126 127 int_tuple = struct.unpack('B'*len(comstr), comstr) 128 divd = self.__bitrev(int_tuple) 129 130 if(l>4): 131 print " This CRC function currently does not support strings > 4 chars" 132 return 0 133 134 divd = self.__bitrev(ord(comstr[0])) 135 136 # put the chars in an integer 137 for i in range(1,l): 138 new = self.__bitrev(ord(comstr[i])) 139 divd <<= 8 140 divd = divd | new 141 142 #crc = 0b10001001<<(8*(l-1)) 143 #hex instead 144 crc = int('10001001',2)<<(8*(l-1)) #J binary literals don't work in python 2.5 145 lsbcheck = 0x80 << (8*(l-1)) 146 147 for i in range(0,8*l): 148 if(divd & lsbcheck == lsbcheck): 149 divd = divd ^ crc 150 divd = divd << 1 151 else: 152 divd = divd<<1 153 154 divd = divd>>(8*(l-1)) 155 divd = self.__bitrev(divd) 156 s = chr(divd & 0xff) 157 158 return comstr + s

159

160 - def __bitrev(self,bytes):

161 """ 162 Creates a lookup table of reversed bit orders 163 164 Input: 165 bytes -- tuple -- tuple of 1 byte values to be reversed 166 Output: 167 bitrev_table -- dict 168 """ 169 bytes = sum(bytes) # Sums the bytes 170 bin_repr = bin(bytes)[2:] # Convert to binary string, remove "0b" at the beginning of the string 171 bin_repr = bin_repr[::-1] # Reverse all digits 172 bin_repr = "0b%s" % bin_repr 173 174 return int(bin_repr,2) # Convert back to int, and return

175

176 177 -class Protocol( AbstractProtocol ):

178 """ 179 This is a concrete class that provides all the 180 functionality needed to send messages to a pololu 181 controller over a "pololu bus". This protocol follows 182 the specifications provided by the Pololu Maestro 183 Documentation found at: 184 http://www.pololu.com/docs/0J40/all 185 186 For use with the Pololu Maestro 12pin, Firmware Version1.1 187 188 It is meant to mimic the can.Protocol class, except 189 for the pololu device rather than a CAN network 190 191 This converts CKBot Module-specific commands into 192 Pololu equivalents, and maintains the state of the 193 Pololu device and its handles to modules via fake heartbeats 194 u 195 WARNING: Current version has only been tested with the 196 12-pin Pololu Maestro Firmwarev1.1 and does NOT support Pololu-styled 197 commands (supports only MiniSSC2 and Compact), support for Pololu-styled 198 commands will be included in a future release perhaps 199 """ 200 201 # Pololu Protocol Sync Value (must be 0xAA) 202 # This is also to initialize the Maestro to begin receiving commands using the PololuProtocol 203 POLOLU_BYTE = 0xAA 204

205 - def __init__(self, bus=None, nodes=None, *args,**kw):

206 """ 207 Initialize a pololu.Protocol 208 209 INPUT: 210 bus -- pololu.Bus -- Serial bus used to communicate with Pololu Device 211 nodes -- dictionary -- key:module node_id, value:pololu controller number 212 213 ATTRIBUTES: 214 heartbeats -- dictionary -- key:nid, value:(timestamp) 215 msgs -- dictionary -- a fake representation of a dictionary message, used so the pololu.Protocol can "dock" onto existing Cluster interfaces (provides the Module version) 216 pna -- dictionary -- table of NodeID to ProtocolNodeAdaptor mappings 217 218 FUTURE: 219 buses -- may be a list of buses (Protocol can communicate with multiple buses by changing servonums) 220 """ 221 AbstractProtocol.__init__(self,*args,**kw) 222 if bus is None: 223 self.bus = Bus() 224 else: 225 self.bus = bus 226 if nodes is None: 227 # By default, map all of the maestro to 0x10..0x1C 228 nodes = dict(zip(range(0x10,0x1C),range(12))) 229 self.nodes = nodes 230 self.heartbeats = {} # Gets populated by update 231 self.msgs = {} 232 self.pnas = {} 233 234 self.pololu_setup() # Must be called before the Maestro can begin to respond to commands

235

236 - def pololu_setup(self):

237 """ 238 Initialize the Pololu Maestro device to receive commands using Pololu Mode 239 """ 240 ##V: This seems to be in an odd location... 241 self.bus.write( (self.POLOLU_BYTE,) )

242

243 - def send_cmd(self, cmd_type, nid, cmd):

244 """ 245 Sends command to the Pololu Maestro via the Bus. 246 247 INPUTS: 248 nid -- int -- Node ID to send the command to 249 cmd -- tuple of ints -- tuple of integer command values to send 250 cmd_type -- int -- Type of command (MiniSSC2, Pololu, and Compact types are supported) 251 """ 252 253 channel = self.nodes[nid] # Extract channel from node ID to channel map 254 255 # Correctly format the command 256 ##V: This seems to be fishy... 257 cmd = list(cmd) 258 cmd.insert(0, cmd_type) 259 cmd.insert(1, channel) 260 cmd = tuple(cmd) 261 262 self.bus.write(cmd)

263

264 - def hintNodes( self, nodes ):

265 """ 266 Specify which nodes to expect on the bus 267 """ 268 self.nodes = set(nodes)

269

270 - def update(self):

271 """ 272 Updates the pololu.Protocol state that mimics the behaviour of can.Protocol. It updates 273 timestamps of heartbeats heard on the bus. 274 """ 275 # sets the of value all the entries in the heartbeats dictionary to the current time 276 # This allows Cluster to believe that all the modules connected through Pololu are alive 277 timestamp = now() 278 dummydata = 0 # dummy data 279 280 for nid in self.nodes.iterkeys(): 281 self.heartbeats[nid] = (timestamp, dummydata) 282 283 # We have no incomplete messages 284 return 0

285

286 - def generatePNA(self, nid):

287 """ 288 Generates a pololu.ProtocolNodeAdaptor, associating a pololu protocol with 289 a specific node id and returns it 290 """ 291 pna = ProtocolNodeAdaptor(self, nid) 292 self.pnas[nid] = pna 293 return pna

294

295 -class Msg(object):

296 """ 297 A concrete class representing a FAKE completed response to a Robotics Bus 298 Dictionary Object request. 299 300 ATTRIBUTES: 301 payload -- partial dictionary object assembled from segments 302 timestamp -- time when full dictionary object response is received 303 incomplete_msg -- contains individual segments of the dictionary object 304 response 305 """

306 - def __init__(self, incomplete_msg, payload, timestamp):

307 self.payload = payload 308 self.timestamp = timestamp 309 self.incomplete_msg = incomplete_msg

310

311 #Belongs in pololu.py -- mimics can.ProtocolNodeAdaptor 312 -class ProtocolNodeAdaptor( AbstractNodeAdaptor ):

313 """ 314 Utilizes the protocol along with nid to create 315 an interface for a specific module 316 """ 317 318 # MiniSSCII Protocol Sync Value (must ALWAYS be 0xFF) specified by Pololu Documentation 319 MINISSC2_BYTE = 0xFF 320 321 # Compact Protocol Sync Value (must be 0x9F) 322 COMPACT_BYTE = 0x8F 323 324 # Pololu Protocol Sync Value (must be 0xAA) 325 # This is also to initialize the Maestro to begin receiving commands using the PololuProtocol 326 POLOLU_BYTE = 0xAA 327 328 ##V: Need to put this in a proper location 329 SLACK_MESSAGE = 0 330

331 - def __init__(self, protocol, nid):

332 self.p = protocol 333 self.nid = nid

334

335 - def go_slack(self):

336 cmd = ( self.SLACK_MESSAGE, ) 337 self.p.send_cmd(self.COMPACT_BYTE, self.nid, cmd)

338

339 - def set_pos(self, target):

340 """ 341 Sends a command to the Pololu device over serial via the 342 pololu.Protocol.send_cmd() 343 344 INPUT: 345 cmd_type -- int -- command type specified by the Pololu User Manual 346 (see pololu.Protocol.send_cmd for more info.) 347 348 data -- int -- the payload data to send to the module 349 """ 350 351 # Essentially passes on the call to the protocol, includes the nid 352 cmd = ( target, ) 353 self.p.send_cmd(self.MINISSC2_BYTE, self.nid, cmd)

354

355 - def get_typecode( self ):

356 return "PolServoModule"

357

358 ## Safety/range values goes in here 359 -class ServoModule( AbstractServoModule ):

360 """ 361 ServoModule Class has the basic functionality of ServoModules, with some exceptions listed below: 362 363 - Pololu Modules cannot get_pos or is_slack 364 - The Pololu Device allows for: 365 - servo parameter settings 366 - set speed 367 - set neutral 368 and various options for setting positions. We currently only use absolute 369 positions however. For more information refer to the Pololu User Manual 370 on p.6 371 """

372 - def __init__(self, node_id, typecode, pna, *argv, **kwarg):

373 AbstractServoModule.__init__(self, node_id, typecode, pna, *argv, **kwarg ) 374 self._attr.update( 375 go_slack="1R", 376 set_pos="2W", 377 get_pos="1R", 378 is_slack="1R" 379 ) 380 381 ##V: How should these be initialized? I just initialize them in set_pos / go_slack for now 382 self.slack = None; # Initialized to True or False 383 self.pos = None; # Initialized to start position of module

384 385 @classmethod

386 - def _deg2pol(cls, angle):

387 """ 388 Returns a correctly scaled module position 389 390 INPUT: 391 angle -- int -- between 9000 to -9000, in 100ths of degrees, 0 is neutral 392 393 OUTPUT: 394 corrected_angle -- int -- scaled between 0 and 255, 127 is neutral 395 """ 396 scale = 1.0*(255-0)/(9000--9000) # Constant scale factor 397 corrected_angle = int(scale*angle + 127) 398 return corrected_angle

399

400 - def is_slack(self):

401 """ 402 Returns true if the module is slack, none if go_slack has not been called yet. 403 404 WARNING: This function does NOT actually read states from the pololu device, returns an attribute that is updated by calls to set_pos and go_slack. If any external communications fail, then this function may report incorrect states 405 """ 406 return self.slack

407

408 - def get_pos(self):

409 """ 410 Returns the 'believed' position of the module, none if set_pos has not been called yet. 411 412 WARNING: This function does NOT actually read states from the pololu device, returns an attribute that is updated by calls to set_pos and go_slack. If any external communications fail, then this function may report incorrect states 413 """ 414 return self.pos

415

416 - def go_slack(self):

417 """ 418 Equivalent of setting a ServoModule slack. This is referred to as "off" 419 as specified in the Pololu User Manual under the Command 0 420 """ 421 # Send the command via the PNA 422 self.pna.go_slack() 423 424 # Module should now be slack 425 self.slack = True

426

427 - def set_pos(self, pos):

428 """ 429 Sets the position of a pololu module. 430 431 Uses the Pololu Set Absolute Position (Command 4) 432 specified by the Pololu User Manual 433 434 INPUT: 435 pos -- int -- the desired position of the module, value between 9000 and -9000 436 437 """ 438 # Ensures value is between 9000 and -9000 439 if pos > 9000 or pos < -9000: 440 raise ValueError("Value out of bounds. Must be between 9000 and -9000.") 441 442 corrected_pos = self._deg2pol(pos) 443 444 # Send Position Command 445 self.pna.set_pos(corrected_pos) 446 447 # Module should now not be slack 448 self.slack = False 449 450 # Module should now be at this position 451 self.pos = pos

452

Source Code for Module ckbot.pololu