1 """
2 -- The ckbot.dynamixel python module implements classes that communicate with CKBot nodes connected
3 to a ( Robotis ) Dynamixel Half-Duplex RS485 Bus, as per Robotis documentation
4
5 Current protocol documentation is found at:
6 http://support.robotis.com/en/product/actuator/dynamixel/dxl_communication.htm
7
8 This library was written referring to the EX-106 manual v1.12
9 which Robotis no longer makes available from its website.
10
11 -- ckbot.dynamixel utilizes Bus and Protocol classes as an interface between the dynamixel hardware
12 and protocol communication layers
13
14 ... talk about meminterface, pna, and module classes ...
15 -- Typical usage via ckbot.logical python module:
16 >>> from ckbot import logical, dynamixel
17 >>> nodes = {0x01:'head', 0x06:'mid', 0x03:'tail'}
18 >>> c = logical.Cluster( dynamixel.Protocol(nodes = nodes.keys()) )
19 >>> c.populate( names=nodes, required=nodes.keys() )
20 >>> c.at.head.set_pos(1000)
21
22 -- Subclass/extend??
23
24 -- CKbot.dynamixel depends upon the following:
25 - ckbot.ckmodule
26 - Module
27 - AbstractBus
28 - AbstractNodeAdaptor
29 - AbstractProtocol
30 - AbstractServoModule
31 - progress
32 - ckbot.port2port
33 - newConnection
34 """
35
36 from time import time as now, sleep
37 from sys import platform as SYS_PLATFORM, stdout
38 from struct import pack, unpack, calcsize
39 from array import array
40 from serial import Serial
41 from subprocess import Popen, PIPE
42 from glob import glob
43 from random import uniform
44 from collections import deque
45
46 from ckmodule import Module, AbstractNodeAdaptor, AbstractProtocol, AbstractBus, progress, AbstractServoModule, AbstractProtocolError, AbstractBusError, MemInterface, MissingModule
47 from port2port import newConnection
48
49 DEFAULT_PORT = dict(TYPE='tty', baudrate=115200, timeout=0.01)
52 "(organizational) Error for Dynamixel Servo """
55
56 -def crop( val, lower, upper ):
57 return max(min(val,upper),lower)
58
59 DEBUG = []
62 """
63 DESCRIPTION:
64 -- The Dynamixel class is a namespace containing useful constants and functions
65 RESPONSIBILITIES:
66 -- Serve as a utility class with dynamixel commands and useful functions
67 OWNERSHIP:
68 -- owned by the dynamixel module
69 THEORY:
70 -- none
71 CONSTRAINTS:
72 -- none ... so far
73 """
74 CMD_PING = 0x01
75 CMD_READ_DATA = 0x02
76 CMD_WRITE_DATA = 0x03
77 CMD_REG_WRITE = 0x04
78 CMD_ACTION = 0x05
79 CMD_RESET = 0x06
80 CMD_SYNC_WRITE = 0x83
81 MEM_LEN = 0x39
82 SYNC = '\xff\xff'
83 MAX_ID = 0xFD
84 BROADCAST_ID = 0xFE
85
88 """
89 DESCRIPTION:
90 Abstract superclass of all DynamixelMemMap classes. Provides
91 shared functionality, while the concrete subclasses provide the
92 actual memory map details.
93
94 NOTE: this is intentionally an "old style class", to ensure that tab completion
95 does not list a gazzilion irrelevant builtin methods.
96
97 RESPONSIBILITIES:
98 -- Provide human readable names for addresses in the dynamixel control table
99 OWNERSHIP:
100 -- owned by the dynamixel module
101 THEORY:
102 -- create a dictionary _ADDR_DCR_ that holds the memory location, name, and type (B, >H)
103 generate a set of properties with the commmand name and it equivalent location
104 """
105 _ADDR_DCR = {
106 '\x00' : ("model", "<H")
107 }
108 MODEL_ADDR = pack('B',0)
109 MODEL_ADDR_LEN = 2
110
111 @classmethod
113 """ (protected)
114 Set up all the _ADDR_DCR names as class attributes
115 This classmethod must be called on every subclass after it is declared
116 """
117 for adr,(nm,fmt) in cls._ADDR_DCR.iteritems():
118 setattr(cls,nm,adr)
119 DynamixelMemMap._prepare()
122 """
123 Mixin class providing utility functions for operating on a MemMap.
124
125 The .mm members of PNA-s use this mixin, whereas the .mcu members of modules do not
126 """
127
128 @classmethod
130 """ Parse the message bytes returned when reading address addr """
131 if isinstance(val,Exception):
132 raise val
133 return unpack( cls._ADDR_DCR[addr][1], val )[0]
134
135 @classmethod
137 """ Build the message bytes sent when writing address addr with value val"""
138 return pack( cls._ADDR_DCR[addr][1], val )
139
140 @classmethod
142 """ Number of message bytes returned when writing address addr """
143 return calcsize( cls._ADDR_DCR[addr][1] )
144
145 @classmethod
146 - def show( cls, addr, val ):
147 """ Provide human readable representation of a value from address addr"""
148 nm,fmt = cls._ADDR_DCR[addr]
149 return "%s = %d" % (nm, unpack(fmt, val)[0])
150
152 """
153 DESCRIPTION:
154 -- The MX64Mem class provides a mapping of the dynamixel control table:
155 as per the MX-64 manual pp. 2-3, section title "Control Table"
156 """
157 _ADDR_DCR = {
158 '\x00' : ("model", "<H"),
159 '\x02' : ("version", "B"),
160 '\x03' : ("ID", "B"),
161 '\x04' : ("baud", "B"),
162 '\x05' : ("ret_delay", "B"),
163 '\x06' : ("cw_angle_limit", "<H"),
164 '\x08' : ("ccw_angle_limit", "<H"),
165 '\x0b' : ("max_temp", "B"),
166 '\x0c' : ("min_voltage", "B"),
167 '\x0d' : ("max_voltage", "B"),
168 '\x0e' : ("max_torque", "<H"),
169 '\x10' : ("status", "B"),
170 '\x11' : ("alarm_LED", "B"),
171 '\x12' : ("alarm_shutdown", "B"),
172 '\x18' : ("torque_en", "B"),
173 '\x19' : ("LED", "B"),
174 '\x1a' : ("D_gain", "B"),
175 '\x1b' : ("I_gain", "B"),
176 '\x1c' : ("P_gain", "B"),
177 '\x1e' : ("goal_position", "<H"),
178 '\x20' : ("moving_speed", "<H"),
179 '\x22' : ("torque_limit", "<H"),
180 '\x24' : ("present_position", "<H"),
181 '\x26' : ("present_speed", "<H"),
182 '\x28' : ("present_load", "<H"),
183 '\x2a' : ("present_voltage", "B"),
184 '\x2b' : ("present_temperature", "B"),
185 '\x2c' : ("registered_instruction", "B"),
186 '\x2e' : ("moving", "B"),
187 '\x2f' : ("lock", "B"),
188 '\x30' : ("punch", "<H"),
189 '\x44' : ("current", "<H"),
190 '\x46' : ("torque_control_mode_enable", "B"),
191 '\x47' : ("goal_torque", "<H"),
192 '\x49': ("goal_acceleration", "B"),
193 }
194 MX64Mem._prepare()
195
196 -class MX28Mem( DynamixelMemMap):
197 """
198 DESCRIPTION:
199 -- The MX28Mem class provides a mapping of the dynamixel control table:
200 http://support.robotis.com/en/product/dynamixel/mx_series/mx-28.htm
201 """
202 _ADDR_DCR = {
203 '\x00' : ("model", "<H"),
204 '\x02' : ("version", "B"),
205 '\x03' : ("ID", "B"),
206 '\x04' : ("baud", "B"),
207 '\x05' : ("ret_delay", "B"),
208 '\x06' : ("cw_angle_limit", "<H"),
209 '\x08' : ("ccw_angle_limit", "<H"),
210 '\x0b' : ("max_temp", "B"),
211 '\x0c' : ("min_voltage", "B"),
212 '\x0d' : ("max_voltage", "B"),
213 '\x0e' : ("max_torque", "<H"),
214 '\x10' : ("status", "B"),
215 '\x11' : ("alarm_LED", "B"),
216 '\x12' : ("alarm_shutdown", "B"),
217 '\x14' : ("multi_turn_offset","<H"),
218 '\x16' : ("resolution_divider","B"),
219 '\x18' : ("torque_en", "B"),
220 '\x19' : ("LED", "B"),
221 '\x1a' : ("D_gain", "B"),
222 '\x1b' : ("I_gain", "B"),
223 '\x1c' : ("P_gain", "B"),
224 '\x1e' : ("goal_position", "<H"),
225 '\x20' : ("moving_speed", "<H"),
226 '\x22' : ("torque_limit", "<H"),
227 '\x24' : ("present_position", "<H"),
228 '\x26' : ("present_speed", "<H"),
229 '\x28' : ("present_load", "<H"),
230 '\x2a' : ("present_voltage", "B"),
231 '\x2b' : ("present_temperature", "B"),
232 '\x2c' : ("registered", "B"),
233 '\x2e' : ("moving", "B"),
234 '\x2f' : ("lock", "B"),
235 '\x30' : ("punch", "<H"),
236 '\x49' : ("goal_acceleration", "B"),
237 }
238 MX28Mem._prepare()
239
240 -class RX64Mem(DynamixelMemMap):
241 """
242 DESCRIPTION:
243 -- The RX64Mem class provides a mapping of the dynamixel control table:
244 as per the RX-64 manual pp. 1-2, section title "Control Table"
245 NOTES:
246 -- Similar to EX106+ but lacks drive mode (for dual module joints) and current sensing.
247 """
248 _ADDR_DCR = {
249 '\x00' : ("model", "<H"),
250 '\x02' : ("version", "B"),
251 '\x03' : ("ID", "B"),
252 '\x04' : ("baud", "B"),
253 '\x05' : ("ret_delay", "B"),
254 '\x06' : ("cw_angle_limit", "<H"),
255 '\x08' : ("ccw_angle_limit", "<H"),
256 '\x0b' : ("max_temp", "B"),
257 '\x0c' : ("min_voltage", "B"),
258 '\x0d' : ("max_voltage", "B"),
259 '\x0e' : ("max_torque", "<H"),
260 '\x10' : ("status", "B"),
261 '\x11' : ("alarm_LED", "B"),
262 '\x12' : ("alarm_shutdown", "B"),
263 '\x18' : ("torque_en", "B"),
264 '\x19' : ("LED", "B"),
265 '\x1a' : ("cw_compliance_margin", "B"),
266 '\x1b' : ("ccw_compliance_margin", "B"),
267 '\x1c' : ("cw_compliance_slope", "B"),
268 '\x1d' : ("ccw_compliance_slope", "B"),
269 '\x1e' : ("goal_position", "<H"),
270 '\x20' : ("moving_speed", "<H"),
271 '\x22' : ("torque_limit", "<H"),
272 '\x24' : ("present_position", "<H"),
273 '\x26' : ("present_speed", "<H"),
274 '\x28' : ("present_load", "<H"),
275 '\x2a' : ("present_voltage", "B"),
276 '\x2b' : ("present_temperature", "B"),
277 '\x2c' : ("registered_instruction", "B"),
278 '\x2e' : ("moving", "B"),
279 '\x2f' : ("lock", "B"),
280 '\x30' : ("punch", "<H"),
281 }
282 RX64Mem._prepare()
285 """
286 DESCRIPTION:
287 -- The EX106Mem class provides a mapping of the dynamixel control table:
288 as per EX-106 section 3-4 pp. 20
289 CONSTRAINTS:
290 -- none???
291 """
292 _ADDR_DCR = {
293 '\x00' : ("model", "<H"),
294 '\x02' : ("version", "B"),
295 '\x03' : ("ID", "B"),
296 '\x04' : ("baud", "B"),
297 '\x05' : ("ret_delay", "B"),
298 '\x06' : ("cw_angle_limit", "<H"),
299 '\x08' : ("ccw_angle_limit", "<H"),
300 '\x0a' : ("drive_mode", "B"),
301 '\x0b' : ("max_temp", "B"),
302 '\x0c' : ("min_voltage", "B"),
303 '\x0d' : ("max_voltage", "B"),
304 '\x0e' : ("max_torque", "<H"),
305 '\x10' : ("status", "B"),
306 '\x11' : ("alarm_LED", "B"),
307 '\x12' : ("alarm_shutdown", "B"),
308 '\x18' : ("torque_en", "B"),
309 '\x19' : ("LED", "B"),
310 '\x1a' : ("cw_compliance_margin", "B"),
311 '\x1b' : ("ccw_compliance_margin", "B"),
312 '\x1c' : ("cw_compliance_slope", "B"),
313 '\x1d' : ("ccw_compliance_slope", "B"),
314 '\x1e' : ("goal_position", "<H"),
315 '\x20' : ("moving_speed", "<H"),
316 '\x22' : ("torque_limit", "<H"),
317 '\x24' : ("present_position", "<H"),
318 '\x26' : ("present_speed", "<H"),
319 '\x28' : ("present_load", "<H"),
320 '\x2a' : ("present_voltage", "B"),
321 '\x2b' : ("present_temperature", "B"),
322 '\x2c' : ("registered_instruction", "B"),
323 '\x2e' : ("moving", "B"),
324 '\x2f' : ("lock", "B"),
325 '\x30' : ("punch", "<H"),
326 '\x38' : ("sense_current","<H"),
327 }
328 EX106Mem._prepare()
332
335
338
341
344
346 """
347 -- The AX-12 control table in
348 http://support.robotis.com/en/product/dynamixel/ax_series/dxl_ax_actuator.htm
349 is identical to that of the RX64, so we re-use that implementation
350 """
351
352 memMapParent = RX64Mem
353
354 -class Bus( AbstractBus ):
355 """ ( concrete )
356 DESCRIPTION:
357 -- The Bus class serves as an interface between the dynamixel RS485 bus and Protocol
358 RESPONSIBILITIES:
359 -- intialize a serial newConnection at the desired baudrate. default: 1000000
360 -- format packets into dynamixel understanble messages
361 -- write packets
362 -- read in valid packets, throw out others
363 - maintain error counts from read attempts
364 -- provide syncronous write then read capability
365 OWNERSHIP:
366 -- owned by the dynamixel module
367 THEORY:
368 -- builds and writes Dynamixel EX packets as per EX-106 section 3-2 pp. 16
369 -- reads packets and parses for valid ones as per EX-106 section 3-3 pp. 17
370 CONSTRAINTS:
371 -- requires valid baudrate setting for communication
372 -- writes and reads limited by pyserial->termios
373 """
374 - def __init__(self, port=None,*args,**kw):
375 """
376 Initialize Dynamixel Bus
377
378 INPUT:
379 port -- serial port string or None to autoconfig
380
381 ATTRIBUTES:
382 ser -- serial handle
383 buf -- buffer string '
384 expect -- number of expected bytes. default: 6
385 eSync -- number of SYNC errors
386 eChksum -- number of checksum errors
387 eLen -- number of length errors
388 eID -- number of ID errors
389 count -- a count of bytes received
390 rxPkts -- a count of valid packets received
391 txPkts -- a count of packets sent
392 """
393 AbstractBus.__init__(self,*args,**kw)
394 if port is None:
395 port = DEFAULT_PORT
396 self.ser = newConnection(port)
397 self.DEBUG = DEBUG
398 self.reset()
399
401
402 self.ser.close()
403 spec = self.ser.newConnection_spec
404 spec.update( changes )
405 self.ser = newConnection( spec )
406 if not self.ser.isOpen():
407 raise ValueError('Could not open newly configured connection')
408
414
416 """
417 The purpose of this method is to reset the state of the Bus to initial values
418 """
419 self.buf = ''
420 self.expect = 6
421 self.eSync = 0
422 self.eChksum = 0
423 self.eLen = 0
424 self.eID = 0
425 self.count = 0
426 self.rxPkts = 0
427 self.rxEcho = 0
428 self.txPkts = 0
429 self.txBytes = 0
430 self.suppress = {}
431 self.ser.flush()
432
434 """
435 Flush software and hardware buffers
436 """
437 self.buf = ''
438 self.ser.flush()
439 if 'x' in self.DEBUG:
440 progress('[Dynamixel] flush bus\n')
441
443 """
444 returns the current Bus statistics
445
446 OUTPUTS:
447 -- list -- strings indicating current counts, error, etc ...
448 """
449 return [
450 'bytes in %d' % self.count,
451 'bytes out %d' % self.txBytes,
452 'packets out %d' % self.txPkts,
453 'packets in %d' % self.rxPkts,
454 'echos in %d' % self.rxEcho,
455 'sync errors %d' % self.eSync,
456 'id errors %d' % self.eID,
457 'length errors %d' % self.eLen,
458 'crc errors %d' % self.eChksum,
459 'buffer %d expecting %d' % (len(self.buf),self.expect)
460 ]
461
463
464 self.ser.close()
465 spec = self.ser.newConnection_spec
466 spec.update( changes )
467 self.ser = newConnection( spec )
468 if not self.ser.isOpen():
469 raise ValueError('Could not open newly configured connection')
470
471 @classmethod
473 """(private)
474 Compute checksum as per EX-106 section 3-2 pp. 17
475
476 INPUTS:
477 dat -- string -- data from which to compute checksum
478 OUTPUTS:
479 return -- int -- value of checksum between 0 and 0xFF
480
481 THEORY OF OPERATION:
482 Compute checksum using not bit operator for only the lowest
483 byte of the sum
484 """
485 return 0xFF ^ (0xFF & sum([ ord(c) for c in dat ]))
486
487 @classmethod
488 - def parseErr( cls, pkt, noRaise=False ):
489 """ (private)
490 Parse and output error returned from dynamixel servos as per 3-4-1 pp. 25
491 """
492 nid = ord(pkt[0])
493 err = ord(pkt[2])
494 msg = ["ID 0x%02x" % nid]
495 if err & 0x01:
496 msg.append("Voltage out of operating range")
497 if err & 0x02:
498 msg.append("Angular position out of range")
499 if err & 0x04:
500 msg.append("Overheating")
501 if err & 0x08:
502 msg.append("Command out of range")
503 if err & 0x10:
504 msg.append("Received message with checksum error")
505 if err & 0x20:
506 msg.append("Current load cannot be controlled with set maximum torque")
507 if err & 0x40:
508 msg.append("Unknown instruction in message")
509 if len(msg)>1 and not noRaise:
510 raise DynamixelServoError("; ".join(msg))
511 return "; ".join(msg)
512
514 """ (private)
515 Drop a single byte from the input buffer .buf and reset .expect
516 If error is provided, increment that error counter
517 """
518 self.buf = self.buf[1:]
519 self.expect = 6
520 if error is not None:
521 setattr(self,error,getattr(self,error)+1)
522
523 @classmethod
524 - def dump( cls, msg ):
525 """
526 Parses a message into human readable form
527 INPUT:
528 msg -- one or more message in a string
529 OUTPUT:
530 the messages in human-readable form
531 """
532 out = []
533 b = [ ord(m) for m in msg ]
534 while len(b)>5:
535 if b[0] != 0xff or b[1] != 0xff:
536 out.append("<<bad SYNC %02X %02X>>" % (b[0],b[1]))
537 else:
538 out.append("<SYNC>")
539 b = b[2:]
540 out.append("nid = 0x%02x" % b[0])
541 l = b[1]
542 if l+1>len(b):
543 out.append("<<bad len = %d total %d>>" % (l,len(b)))
544 break
545 out.append("len = %d" % l)
546 out.append({ 0x02:"read", 0x03:"write", 0x01:"ping",
547 0x04:"REG WRITE", 0x05:"ACTION", 0x06:"RESET",
548 0x83:"SYNC WRITE"
549 }.get(b[2],"<<CMD 0x%02x>>" % b[2]))
550
551 if b[2]!=0x83:
552 out.append(" ".join("%02X" % bb for bb in b[3:l+1]) )
553 else:
554 out.append('addr = 0x%02X for %d' % (b[3],b[4]))
555 for k in xrange(5,l,3):
556 out.append('nid %02X %02X %02X' % tuple(b[k:k+3]))
557 chk = sum(b[:l+1])
558 if 0xFF & (b[l+1]+chk) != 0xFF:
559 out.append("<<bad CHK %02X, need %02X>>" %(b[l+1],0xFF^chk))
560 else:
561 out.append("<CHK>")
562
563 b=b[l+2:]
564 return " ".join(out)
565
567 SWEEP_EVERY = 100
568 tbl = self.suppress
569 n = self.txPkts
570
571 if (n % SWEEP_EVERY) == 0:
572
573
574 for key in tbl.keys():
575 if tbl[key] < n - SWEEP_EVERY:
576 del tbl[key]
577
578 if tbl.has_key(pkt):
579
580 del tbl[pkt]
581 return True
582 return False
583
584 - def recv( self, maxlen=10 ):
585 """
586 reads a single packet off of the RS485 or serial bus
587
588 INPUTS:
589 None
590 OUTPUTS:
591 pkt -- string -- valid packet from Dynamixel hardware bus
592 PRECONDITIONS:
593 existence of serial object
594 reset() has been called at least once
595 POSTCONDITIONS:
596 pkt is returned given that all values of packet check OUTPUTS
597
598 THEORY OF OPERATION:
599 Parse for valid packets as per EX-106 section 3-3 pp. 17
600 - While there are bytes to be read from serial buffer or from buf
601 - Read in expected number of bytes if available
602 - Check for start frame, otherwise drop byte
603 - Check for valid ID, otherwise drop byte
604 - Check for valid Length, otherwise drop byte
605 - Once Length bytes arrive, check if CRC is valid, otherwise drop byte
606 - If there are no more bytes to read or parse then return None
607 """
608 SYNC = Dynamixel.SYNC
609 MAX_ID = Dynamixel.MAX_ID
610 assert self.expect >= 6
611 while len(self.buf)+self.ser.inWaiting()>=self.expect:
612
613
614
615 rd = self.ser.read( self.ser.inWaiting() )
616 self.buf += rd
617 self.count += len(rd)
618
619 if not self.buf.startswith(SYNC):
620
621 self._dropByte('eSync')
622 continue
623
624
625 ID = ord(self.buf[2])
626 if ID > MAX_ID:
627 self._dropByte('eID')
628 continue
629
630 L = 4+ord(self.buf[3])
631
632 if L > maxlen or L < 6:
633 self._dropByte('eLen')
634 continue
635 if len(self.buf)<L:
636 self.expect = L
637 continue
638 assert len(self.buf)>=L
639 chk = self._chksum( self.buf[2:L-1] )
640
641 if ord(self.buf[L-1]) != chk:
642 self._dropByte('eChksum')
643 continue
644
645
646
647 pkt = self.buf[2:L-1]
648 fl_pkt = self.buf[:L]
649 self.buf = self.buf[L:]
650
651 if self._testForEcho(fl_pkt):
652 self.rxEcho += 1
653 if 'x' in self.DEBUG:
654 progress('[Dynamixel] recv echo --> [%s] %s\n' % (self.dump(fl_pkt),repr(fl_pkt)))
655 continue
656 self.rxPkts += 1
657 if 'x' in self.DEBUG:
658 progress('[Dynamixel] recv --> [%s] %s\n' % (self.dump(fl_pkt),repr(fl_pkt)))
659
660 self.parseErr(pkt)
661 return pkt
662
663
664 return None
665
666 - def send( self, nid, cmd, pars = '' ):
667 """
668 Build a message and transmit, update rxPkts, and return message string
669
670 INPUTS:
671 nid -- int -- node ID of module
672 cmd -- int -- command value. ie: CMD_PING, CMD_WRITE_DATA, etc ...
673 pars -- string -- parameter string
674 OUTPUTS:
675 msg -- string -- transmitted packet minus sync
676 PRECONDITIONS:
677 existance of serial object
678 POSTCONDITIONS:
679 None
680
681 THEORY OF OPERATION:
682 Assemble packet as per EX-106 section 3-2 pp. 16,
683 Compute checksum, transmit and then return string
684
685 """
686 body = pack('BBB',nid,len(pars)+2,cmd)+pars
687 msg = Dynamixel.SYNC+body+pack("B",self._chksum(body))
688 if 'x' in self.DEBUG:
689 progress('[Dynamixel] send --> [%s] %s\n' % (self.dump(msg),repr(msg)))
690 self.ser.write(msg)
691 self.txPkts+=1
692 self.txBytes+=len(msg)
693 self.suppress[msg] = self.txPkts
694 return msg[2:]
695
697 """
698 Build a SYNC_WRITE message writing a value
699
700 INPUTS:
701 nid -- int -- node ID of module
702 addr -- string -- address
703 pars -- string -- value (after marshalling)
704 OUTPUTS:
705 msg -- string -- transmitted packet minus sync
706 PRECONDITIONS:
707 existance of serial object
708 POSTCONDITIONS:
709 None
710
711 THEORY OF OPERATION:
712 Set a value remotely, without expecting a reply
713 """
714 body = ( pack('BBB', Dynamixel.BROADCAST_ID, len(pars)+5, Dynamixel.CMD_SYNC_WRITE)
715 +addr
716 +pack('BB', len(pars),nid)
717 +pars
718 )
719 msg = Dynamixel.SYNC+body+pack("B",self._chksum(body))
720 if 'x' in self.DEBUG:
721 progress('[Dynamixel] sync_write --> [%s] %s\n' % (self.dump(msg),repr(msg)))
722 self.ser.write(msg)
723 self.suppress[msg] = self.txPkts
724 self.txPkts+=1
725 self.txBytes+=len(msg)
726 return msg[2:]
727
728 - def ping( self, nid ):
729 """
730 Send a low level ping command to a given nid
731
732 INPUTS:
733 nid -- int -- node ID of module
734 OUTPUTS:
735 None
736 PRECONDITIONS:
737 instantiation of dynamixel.Bus
738 POSTCONDITIONS:
739 No response occurs
740
741 THEORY OF OPERATION:
742 Send CMD_PING for given nid as per section 3-5-5 pp. 37
743 """
744 return self.send(nid, Dynamixel.CMD_PING)
745
747 """
748 Send a low level reset command to a given nid
749
750 INPUTS:
751 nid -- int -- node ID of servo to reset
752
753 THEORY OF OPERATION:
754 Send CMD_RESET for given nid as per section 3-5-6 pp. 38
755 Resets all values in the servo's control table to factory
756 defaults, INCLUDING the ID, which is reset to 0x01.
757
758 By convention, we reserve ID 0x01 for configuration purposes
759 """
760 return self.write(nid, Dynamixel.CMD_RESET)
761
762 - def send_cmd_sync( self, nid, cmd, pars, timeout=0.1, retries=5 ):
763 """
764 Send a command in synchronous form, waiting for reply
765
766 INPUTS:
767 nid, cmd, pars -- same as for .send()
768 timeout -- float-- maximal wait per retry
769 retries -- number of allowed retries until giving up
770 rate -- wait time between retries
771 OUTPUTS:
772 pkt -- string -- response packet's payload
773
774 THEORY OF OPERATION:
775 Plan out sending times for all retries. When a retry goal time is passed
776 sends out the message. In between, poll for a reply for that message at a
777 hard coded rate (currently 100Hz)
778 """
779 if nid==Dynamixel.BROADCAST_ID:
780 raise ValueError('Broadcasts get no replies -- cannot send_cmd_sync')
781 for k in xrange(retries+1):
782 hdr0 = self.send(nid, cmd, pars)[0]
783 t0 = now()
784
785 sleep( 0.003 )
786 while now()-t0<timeout:
787 pkt = self.recv()
788
789 if pkt is None:
790 sleep( 0.001 )
791
792 elif pkt[0]==hdr0:
793 if 't' in self.DEBUG:
794 progress("[Dynamixel] send_cmd_sync dt: %2.5f " % (now()-t0))
795 progress("[Dynamixel] send_cmd_sync send attempts: %d " % (k+1))
796 return pkt
797
798 return None
799
800 @staticmethod
802 """
803 Unpack reply header and payload
804 OUTPUT: nid, len, cmd, tail
805 nid -- node ID
806 len -- message length
807 cmd -- command code in message
808 tail -- remainder of the message, as a string
809 """
810 return unpack('BBB',reply[:3])+(reply[3:],)
811
815
824
826 """
827 Read the typecode and update the mm sub-object to one of the appropriate type
828 """
829 tc = self.get_typecode()
830 try:
831 self.mm = (MODELS[tc][0])
832 except KeyError, ke:
833 raise KeyError('Unknown module typecode "%s"' % tc)
834
836 """
837 Send a memory write command expecting no response.
838 The write occurs immediately once called.
839
840 INPUTS:
841 addr -- char -- address
842 val -- int -- value
843 OUTPUTS:
844 msg -- string -- transmitted packet minus SYNC
845
846 THEORY OF OPERATION:
847 Call write command with BROADCAST_ID as id, SYNC_WRITE as cmd, and params
848 as ( start_address, length_of_data, nid, data1, data2, ... ) as per
849 section 3-5-7 pp. 39
850 """
851 return self.p.mem_write( self.nid, addr, self.mm.val2pkt( addr, val ) )
852
854 """
855 Send a memory write command and wait for response, returning it
856
857 INPUTS:
858 addr -- char -- address
859 val -- int -- value
860 OUTPUTS:
861 msg -- string -- transmitted packet minus SYNC
862 """
863 return self.p.mem_write_sync( self.nid, addr, self.mm.val2pkt( addr, val ))
864
866 """
867 TODO
868 """
869 reply = self.p.mem_read_sync( self.nid, addr, self.mm.val2len(addr) )
870 return self.mm.pkt2val( addr, reply )
871
873 """
874 TODO
875 """
876 if self.model is not None:
877 return self.model
878 mdl = self.mem_read_sync( self.mm.model )
879 if isinstance(mdl,Exception):
880 raise mdl
881 self.model = "Dynamixel-%04x" % mdl
882 return self.model
883
885 """
886 Send a request memory read command expecting response later after p.upate() is called
887
888 INPUTS:
889 addr -- char -- address
890
891 OUTPUTS:
892 promise -- list -- promise returned from request
893 """
894 return self.p.request( self.nid, Dynamixel.CMD_READ_DATA, addr+pack('B',self.mm.val2len(addr)))
895
897 """
898 Send a request memory write command expecting response later after p.update() is called
899
900 INPUTS:
901 addr -- char -- address
902 val -- int -- value to write
903
904 OUTPUTS:
905 promise -- list -- promise returned from request
906 """
907 return self.p.request( self.nid, Dynamixel.CMD_WRITE_DATA, addr+self.mm.val2pkt( addr, val ))
908
909 @classmethod
911 """
912 Parse the result of an asynchronous get.
913 INPUTS:
914 promise -- list -- promise to parse for
915 barf -- bool -- throw exception or not
916
917 OUTPUTs:
918 dat -- string -- data, ProtocolError, or None returned from promise
919
920 THEORY OF OPERATION:
921 When barf is True, this may raise a deferred exception if the
922 operation timed out or did not complete. Otherwise the exception
923 object will be returned.
924 """
925 if not promise:
926 raise ProtocolError("Asynchronous operation did not complete")
927 dat = promise[0]
928 if barf and isinstance(dat,Exception):
929 raise dat
930 return dat
931
933 """
934 Get present voltage on bus as read by servo
935
936 OUTPUTS:
937 -- voltage -- int -- volts
938 THEORY OF OPERATION:
939 -- mem_read the present_voltage register and convert
940 """
941 return self.dynamixel2voltage(self.mem_read(self.mm.present_voltage))
942
944 """
945 Get present temperature of servo
946
947 OUTPUTS:
948 -- temperature -- int -- degrees celsius
949 THEORY OF OPERATION:
950 -- mem_read the present_temperature register
951 """
952 return self.mem_read(self.mm.present_temperature)
953
955 """
956 Get present load
957
958 OUTPUTS:
959 -- load -- int -- a ratio of max torque, where max torque is 0x3FF, needs to be determined!!! -U
960 THEORY OF OPERATION:
961 -- mem_read the present_load register
962 """
963 return self.dynamixel2voltage(self.mem_read(self.mm.present_load))
964
966 """ ( internal concrete )
967 DESCRIPTION:
968 -- The Request message class represents holds information about a message
969 request
970 RESPONSIBILITIES:
971 -- hold nid, cmd, and params of message
972 -- keep track of retries remaining
973 -- hold a timestamp
974 -- hold a promise to be updated after dynamixel.Protocol.update()
975 OWNERSHIP:
976 -- owned by the dynamixel module
977 THEORY:
978 -- An Request object serves a way to keep track of requests. In the case
979 of the dynamixel.Protocol it is appended to a queue and handled at update
980 CONSTRAINTS:
981 -- Request objects are specific to those messages handled by the
982 dynamixel.Protocol and dynamixel.Bus
983 """
984 - def __init__(self, nid, cmd, pars='', ts=None, tout=0.05, attempts=4 ):
985 if ts is None:
986 ts = now()
987 self.nid = nid
988 self.cmd = cmd
989 self.pars = pars
990 self.ts = ts
991 self.tout = tout
992 self.attempts = attempts
993 self.promise = []
994
996 self.promise[:] = [ProtocolError("%s on node 0x%x" % (msg,self.nid))]
997
999 self.promise[:] = [dat]
1000
1002 return self.nid, self.cmd, self.pars
1003
1005 """ ( concrete )
1006 DESCRIPTION:
1007 -- The Protocol class provides the functionality needed to send messages using the
1008 Dynamixel Protocol as per section 3 of the EX-106 document
1009 RESPONSIBILITIES:
1010 -- detect modules on the bus and generate NodeAdaptors for each one
1011 -- ping modules periodically to ensure that they are still on the Bus??
1012 -- store a queue of pending messages and handle them for during each update
1013 for a given timeslice
1014 -- act as a abstraction layer for basic Bus functionality?
1015 OWNERSHIP:
1016 -- owned by the Cluster
1017 THEORY:
1018 -- The protocol owns a dictionary of the NodeAdaptor of each module
1019 It initially checks for modules by performing a scan for all nodes
1020 that the user states should be on the Bus and for each module
1021 that exists it updates the heartbeat dictionary entry for that node
1022
1023 A heartbeat, is a simple indicator of node existing, holding only a
1024 timestamp and the error received.
1025
1026 Message transmission and reception is based upon the following ideas:
1027 - If asked, Dynamixel servos always send a response
1028 - Dynamixel servos have no Bus arbitration, therefore the response
1029 to a request always follows the request
1030
1031 - There are three types of requests
1032 - Writes that require an acknowledgement
1033 - Reads that require an acknowledgement
1034 - Writes that do not require an acknowledgement
1035
1036 The aformentioned ideas led to the following decisions:
1037 - Messages should be placed in a queue and handled in order of requests
1038 - Message that do not require a response should be sent using the
1039 "SYNC_WRITE" command as per section 3-5-7 pp. 39 in the EX-106 document
1040 and no response should be expected.
1041 - Messages that require a response should be sent and listened for. If a
1042 response is not found, a number of retries should occur to accomodate
1043 for errors
1044
1045 A basic representation of the update() algorithm follows:
1046
1047 get the allowed timeslice
1048 while len queue > 0:
1049 pop message from queue
1050 if message has timed out:
1051 declare message promise as Err
1052 if allowed timeslice is used up:
1053 append message to front of queue and exit
1054 if the message is a broadcast message:
1055 if ping:
1056 warn that broadcast pings are not allowed outside of scan
1057 else:
1058 send message through Bus, and continue to next message
1059 else:
1060 for number of retries remaining in message:
1061 if allowed timeslice is used up:
1062 append message to front of queue and exit
1063 send message through Bus
1064 while have not reached timeout or timeslice is used up:
1065 read in packet
1066 if packet exists:
1067 fulfill message promise, and continue to next message
1068 sleep so that we don't overwork the OS
1069 if no more retries remaining:
1070 declare message promise as Err
1071
1072 CONSTRAINTS:
1073 -- a single write when handled by update must have only a single response
1074 -- it is expected that a Dynamixel Bus has been instantiated as shown in the
1075 Dynamixel Module Examples
1076 """
1077 - def __init__(self, bus=None, nodes=None, *args,**kw):
1078 """
1079 Initialize a Dynamixel Protocol
1080
1081 INPUTS:
1082 bus -- object -- dynamixel bus object used for basic communication with dynamixel servos
1083 nodes -- list -- list of expected dynamixel servo IDs
1084
1085 ATTRIBUTES:
1086 pnas -- dictionary -- dict of NodeAdaptor objects, used for interfacing with Dynamixel Module
1087 heartbeats -- dictionary -- dict of node heartbeats. nid : (timestamp, err)
1088 requests -- deque object -- queue of pending requests <<< maybe should just be a list?
1089 ping_period -- float -- expected ping period
1090
1091 """
1092 AbstractProtocol.__init__(self,*args,**kw)
1093 if bus is None:
1094 self.bus = Bus()
1095 else:
1096 self.bus = bus
1097 self.reset(nodes)
1098
1099 - def reset( self, nodes=None, ping_rate=1.0 ):
1100 """
1101 Reset the Protocol object
1102
1103 INPUTS:
1104 ping_rate -- float -- rate at which to ping nodes
1105 OUTPUTS:
1106 None
1107 THEORY OF OPERATION:
1108 scan for nodes, add each node to the pollRing and generate a ProtocolNodeAdaptor
1109 """
1110 self.pnas = {}
1111 self.heartbeats = {}
1112 self.requests = deque()
1113 self.pollRing = deque()
1114 self.ping_rate = ping_rate
1115 if nodes is None:
1116 progress("Scanning bus for nodes \n")
1117 nodes = self.scan()
1118 for nid in nodes:
1119 self.pollRing.append( nid )
1120 self.generatePNA( nid )
1121 progress("Dynamixel nodes: %s\n" % repr(list(nodes)))
1122
1123 - def scan( self, timeout=1.0, retries=1, get_model=True ):
1124 """
1125 Build a broadcast ping message and then read for responses, and if
1126 the get_model flag is set, then get model numbers for all existing
1127 nodes
1128
1129 INPUTS:
1130 timeout -- float -- amount of time allowed to scan
1131 retries -- int -- number of times allowed to retry scan
1132 get_model -- bool -- flag determining if node model numbers
1133 should be read to indicate the servo
1134 type
1135 OUTPUTS:
1136 found -- dict -- map from servo IDs that were discovered
1137 to their model numbers, or None if get_model is False
1138
1139 THEORY OF OPERATION:
1140 Assemble packet as per EX-106 section 3-2 pp. 16, using
1141 PING Command as per section 3-5-5 pp. 37 and broadcast ID
1142 Send this packet, then read responses and add to set until
1143 timeout and number of retries.
1144 """
1145 found = []
1146 for retry in xrange(0, retries):
1147 self.bus.flush()
1148 self.bus.send(Dynamixel.BROADCAST_ID, Dynamixel.CMD_PING)
1149 t0 = now()
1150 while now()-t0 < float(timeout)/retries:
1151 pkt = self.bus.recv()
1152 if pkt is not None:
1153 nid = unpack('B',pkt[0])[0]
1154 found.append(nid)
1155 sleep(0.01)
1156 res={}
1157 for nid in found:
1158 if get_model:
1159 res[nid] = self.mem_read_sync( nid, DynamixelMemMap.MODEL_ADDR, DynamixelMemMap.MODEL_ADDR_LEN)
1160 else:
1161 res[nid] = None
1162 return res
1163
1165 """
1166 Generate NodeAdaptors for nodes that are not already in pnas
1167
1168 INPUTS:
1169 nodes -- list -- list of expected nodes
1170
1171 THEORY OF OPERATION:
1172 Get all node IDs that are in nodes but not in pnas and
1173 generate a NodeAdaptors for those nodes
1174 """
1175 n1 = set(nodes)
1176 n0 = set(self.pnas.keys())
1177 for nid in n0-n1:
1178
1179 self.pnas[nid].nid = "<<INVALID>>"
1180 del self.pnas[nid]
1181 for nid in n1-n0:
1182 self.generatePNA(nid)
1183
1185 """
1186 Generate NodeAdaptors for given Node ID
1187
1188 INPUTS:
1189 nid -- int -- ID of node to generate ProtocolNodeAdaptor object for
1190 OUTPUTS:
1191 pna -- object -- ProtocolNodeAdaptor object for given nid
1192
1193 THEORY OF OPERATION:
1194 Instantiate ProtocolNodeAdaptor add to pnas dictionary
1195 """
1196 pna = ProtocolNodeAdaptor(self, nid)
1197 self.pnas[nid] = pna
1198 return pna
1199
1201 """
1202 Turn all servos off ... see Bus.off
1203 """
1204 self.bus.off()
1205
1207 """
1208 Send a memory write command and don't wait for a response
1209
1210 INPUTS:
1211 addr -- char -- address
1212 val -- int -- value
1213 pars -- string -- parameters
1214 OUTPUTS:
1215 msg -- string -- transmitted packet minus SYNC
1216 """
1217 return self.bus.send_sync_write( nid, addr, pars )
1218
1220 """
1221 Send a memory write command and wait for response, returning it
1222
1223 INPUTS:
1224 addr -- char -- address
1225 val -- int -- value
1226 pars -- string -- parameters
1227 OUTPUTS:
1228 msg -- string -- transmitted packet minus SYNC
1229 """
1230 return self.bus.send_cmd_sync( nid, Dynamixel.CMD_WRITE_DATA, addr+pars )
1231
1233 """
1234 A Protocol level syncronous memory read wrapper around bus.send_cmd_sync
1235
1236 INPUTS:
1237 nid -- int -- node ID
1238 addr -- string hex -- address location to read from
1239 length -- int -- number of bytes to read
1240 retries -- int -- number of times to retry if there exist protocol errors
1241 """
1242 for retry in xrange(0,retries):
1243 reply = self.bus.send_cmd_sync( nid, Dynamixel.CMD_READ_DATA, addr+pack('B', length))
1244 if reply is None:
1245 return ProtocolError("NID 0x%02x mem_read[0x%02x] timed out"
1246 % (nid, ord(addr)))
1247 res = self.bus.splitReply(reply)[-1]
1248 if len(res) != length:
1249 if retry < retries:
1250 continue
1251 return ProtocolError("NID 0x%02x mem_read[0x%02x] result length mismatch. Reply was %s" % (nid,ord(addr),repr(reply)))
1252 return res
1253
1254 - def request( self, nid, cmd, pars='', lifetime=0.05, **kw ):
1255 """
1256 Send a response request by creating an incomplete messsage
1257 and appending it to the queue for handling by update
1258
1259 INPUTS:
1260 nid -- int -- node ID of module
1261 cmd -- int -- command value. ie: CMD_PING, CMD_WRITE_DATA, etc ...
1262 pars -- string -- parameter string
1263 OUTPUTS:
1264 promise -- list -- promise of incomplete message, will be fulfilled upon
1265 successful read
1266 PRECONDITIONS:
1267 None?
1268 POSTCONDITIONS:
1269 None?
1270
1271 THEORY OF OPERATION:
1272 Create an incomplete message for a given request that holds the
1273 packet to be written and has a number of other attributes ( see
1274 Request ) Then append to Protocols queue for later handling
1275 and return a promise that will be fulfilled by a successful read
1276 during Protocol.update()
1277 """
1278 inc = Request( nid, cmd, pars, tout=lifetime)
1279 self.requests.append( inc )
1280 return inc.promise
1281
1283 """
1284 Use pollRing to poll all nodes that haven't been seen for
1285 self.ping_rate seconds with ping packets.
1286
1287 INPUTS:
1288 now -- float -- current time
1289 """
1290 nxt = deque()
1291 while self.pollRing:
1292 nid = self.pollRing.popleft()
1293 nxt.append(nid)
1294 if now-self.heartbeats.get( nid, (0,) )[0]>self.ping_rate:
1295 self.request( nid, Dynamixel.CMD_PING )
1296 break
1297 self.pollRing.extend(nxt)
1298
1299 - def update( self, timeout=0.01 ):
1300 """
1301 The update method handles current incomplete message requests and ensures that
1302 nodes are pinged to check for their existance
1303
1304 INPUTS:
1305 None
1306 OUTPUTS:
1307 num_requests -- int -- length of request queue
1308 PRECONDITIONS:
1309 instantiation of dynamixel.Bus, dynamixel.Protocol
1310 POSTCONDITIONS:
1311 No response occurs
1312
1313 THEORY OF OPERATION:
1314 get the allowed timeslice
1315 while timeslice not done:
1316 if queue empty --> return
1317 pop 1st message on queue
1318 if message has timed out:
1319 fill promise with ProtocolError(timeout)
1320 continue
1321 if message ID is BROADCAST_ID:
1322 if message cmd not CMD_SYNC_WRITE:
1323 bus.send the message
1324 fill promise with None
1325 else:
1326 fill promise with ProtocolError(not CMD_SYNC_WRITE bcast not allowed)
1327 continue
1328 else:
1329 bus.send_cmd_sync the message
1330 store reply in promise
1331 """
1332 t0 = now()
1333 self._get_heartbeats(t0)
1334 t1 = t0
1335 while (t1-t0 < timeout) and self.requests:
1336 t1 = now()
1337 self._doRequest( t0, self.requests.popleft() )
1338
1339
1340 while (t1-t0 < timeout):
1341 if not self.bus.recv():
1342 break
1343
1344
1346 """
1347 Process a single incomplete request
1348 If it requires a response -- complete it or time-out; if not,
1349 complete with a None reply
1350
1351 TODO finish comment
1352 """
1353 if inc.nid == Dynamixel.BROADCAST_ID:
1354 if cmd != Dynamixel.CMD_SYNC_WRITE:
1355 self.bus.send(*inc.sendArgs())
1356 inc.setResponse(None)
1357 else:
1358 inc.setError("broadcast allowed only for CMD_SYNC_WRITE")
1359 else:
1360 reply = self.bus.send_cmd_sync(*inc.sendArgs())
1361 if reply is not None and not isinstance(reply,Exception):
1362 self.heartbeats[inc.nid] = (now, reply)
1363 inc.setResponse(reply)
1364
1366 """ concrete class DynamixelModule provides shared capabilities of
1367 Dynamixel modules. It is usually used as the base class in a mixin,
1368 with the model specific details provided by the mixin classes.
1369 """
1370
1371 MX_POS = 10000
1372 MN_POS = -10000
1373
1374 @classmethod
1376 """ Convert angle values to Dynamixel units """
1377 return int(ang * cls.SCL + cls.OFS)
1378
1379 @classmethod
1381 """Convert dynamixel units to Dynamixel angles"""
1382 return int((dynamixel-cls.OFS) / cls.SCL)
1383
1384 @classmethod
1386 """ Convert "torque" commands to Dynamixel commands """
1387 if torque < 0:
1388 return int(abs(torque)*cls.TORQUE_SCL) | cls.DIRECTION_BIT
1389 else:
1390 return int(torque*cls.TORQUE_SCL)
1391
1392 @classmethod
1400
1401 @classmethod
1403 """Convert rpm to dynamixel units"""
1404 return int(rpm/cls.SPEED_SCL)+1
1405
1406 @classmethod
1410
1411 - def __init__( self, node_id, typecode, pna ):
1412 """
1413 Concrete constructor.
1414
1415 ATTRIBUTES:
1416 node_id -- int -- address for a unique module
1417 typecode -- version name of module
1418 pna -- ProtocolNodeAdaptor -- specialized for this node_id
1419 """
1420 AbstractServoModule.__init__(self, node_id, typecode, pna)
1421 self.mcu = self.pna.mm.memMapParent
1422 self.mem = MemInterface( self )
1423 self._attr.update(
1424 set_pos_sync="2W",
1425 set_trim="2W",
1426 get_trim="1R",
1427 set_speed="2W",
1428 set_speed_sync="2W",
1429 set_torque="2W",
1430 get_speed="1R",
1431 get_voltage="1R",
1432 get_temperature="1R",
1433 get_load="1R",
1434 mem_write="2W",
1435 mem_read="1R",
1436 get_mode="1R",
1437 set_mode="2W",
1438 go_slack="2W"
1439 )
1440 self.get_mode()
1441
1442 - def set( self, **args ):
1443 """Syntactic sugar for setting a bunch of addresses in the dynamixel control table to new values
1444
1445 Keyword arguments may be any of the addresses defined in the .mcu member, e.g. for EX106
1446
1447 TODO: example
1448 """
1449 for nm,val in args.iteritems():
1450 if hasattr( self.mcu,nm ):
1451 self.mem_write( getattr( self.mcu, nm ), val )
1452 else:
1453 raise KeyError("Unknown module address '%s'" % nm)
1454
1456 "Write a byte to a memory address in the module's microcontroller"
1457 self.pna.mem_write_sync(addr, val)
1458
1460 "Read a memory address from the module's microncontroller"
1461 return self.pna.mem_read_sync( addr )
1462
1464 "Return a getter function for a memory address"
1465
1466 def getter():
1467 return self.mem_read(addr)
1468 return getter
1469
1471 "Return a setter function for a memory address"
1472
1473 def setter(val):
1474 return self.mem_write(addr,val)
1475 return setter
1476
1478 """
1479 Get the current mode of the dynamixel servo, either Motor or Servo
1480
1481 OUTPUTS:
1482 mode -- string -- 1 if Motor; 0 if Servo
1483 """
1484 cw_limit = self.pna.mem_read_sync(self.mcu.cw_angle_limit)
1485 ccw_limit = self.pna.mem_read_sync(self.mcu.ccw_angle_limit)
1486 if cw_limit==0 and ccw_limit==0:
1487 self.mode = 1
1488 else:
1489 self.mode = 0
1490 return self.mode
1491
1493 """ ( MAYBE )
1494 Set the current mode of the servo, either Motor or Servo
1495
1496 INPUTS:
1497 mode -- string -- either Motor or Servo
1498 pos_upper -- int -- upper range of servo
1499 pos_lower -- int -- lower range of servo
1500 """
1501 if mode in [0,'Servo']:
1502 pos_upper = self.ang2dynamixel(self.MX_POS)
1503 pos_lower = self.ang2dynamixel(self.MN_POS)
1504 self.mode = 0
1505 elif mode in [1,'Motor']:
1506 pos_upper = 0
1507 pos_lower = 0
1508 self.mode = 1
1509 else:
1510 raise ValueError("Unknown mode %s requested" % repr(mode))
1511 self.mem_write(self.mcu.ccw_angle_limit, pos_upper)
1512 self.mem_write(self.mcu.cw_angle_limit, pos_lower)
1513
1514 - def start( self, null=None ):
1515 """
1516 Enable module actuation
1517
1518 THEORY OF OPERATION:
1519 -- set torque_en as per section 3-4-2 pp. 27
1520 """
1521 return self.mem_write(self.mcu.torque_en, 1)
1522
1524 """
1525 Disable module actuation
1526
1527 THEORY OF OPERATION:
1528 -- clear torque_en. as per section 3-4-2 pp. 27
1529 """
1530 return self.mem_write(self.mcu.torque_en, 0)
1531
1533 """
1534 Disable module actuation
1535
1536 THEORY OF OPERATION:
1537 -- clear torque_en. as per section 3-4-2 pp. 27
1538 """
1539 return self.mem_write(self.mcu.torque_en, 0)
1540
1542 """
1543 Gets the actual position of the module
1544 """
1545 dxl = self.mem_read(self.mcu.present_position)
1546 return self.dynamixel2ang(dxl)
1547
1549 """
1550 <<Disabled>>
1551 """
1552 pass
1553
1555 """
1556 Sets position of the module, with safety checks.
1557
1558 INPUT:
1559 val -- units in 1/100s of degrees between -10000 and 10000
1560 """
1561 return self.pna.mem_write_fast(self.mcu.goal_position,
1562 self.ang2dynamixel(val))
1563
1565 """
1566 Sets position of the module, with safety checks.
1567
1568 INPUT:
1569 val -- units in 1/100s of degrees between -10000 and 10000
1570 """
1571 return self.mem_write(self.mcu.goal_position,
1572 self.ang2dynamixel(val))
1573
1574
1576 """
1577 Get present speed of servo
1578
1579 OUTPUTS:
1580 -- speed -- float -- speed in rpm
1581 THEORY OF OPERATION:
1582 -- mem_read the present_speed register and convert
1583 """
1584 spd = self.mem_read(self.mcu.present_speed)
1585 rpm = self.dynamixel2rpm(spd)
1586 return rpm
1587
1589 """
1590 Get the moving speed (set in set_speed) of servo
1591
1592 OUTPUTS:
1593 -- speed -- float -- speed in rpm
1594 THEORY OF OPERATION:
1595 -- mem_read the moving_speed register and convert
1596 """
1597 spd = self.mem_read(self.mcu.moving_speed)
1598 rpm = self.dynamixel2rpm(spd)
1599 return rpm
1600
1602 """
1603 Sets torque of the module, with safety checks.
1604
1605 INPUT:
1606 val -- units in between -1.0 - 1.0
1607 """
1608 if self.mode == 0:
1609 self.get_mode()
1610 if self.mode == 0:
1611 raise TypeError('set_torque not allowed for modules in Servo')
1612 val = self.torque2dynamixel(val)
1613 return self.pna.mem_write_fast(self.mcu.moving_speed, val)
1614
1616 """
1617 Sets the maximum torque limit of the module
1618 INPUT:
1619 val -- unit from 0.0 to 1.0 where 1.0 is the maximum torque
1620 """
1621 if val > 1.0:
1622 val = 1.0
1623 elif val < 0.0:
1624 val = 0.0
1625 val = int(val*1023)
1626 return self.mem_write( self.mcu.torque_limit, val )
1627
1628
1630 """
1631 Sets speed of the module, with safety checks.
1632
1633 INPUT:
1634 val -- units in rpm from -114 to 114
1635 """
1636 if self.mode == 1:
1637 raise TypeError('set_speed not allowed for modules in CR mode')
1638 val = self.rpm2dynamixel(val)
1639 return self.mem_write(self.mcu.moving_speed, val)
1640
1642 """
1643 Get present voltage on bus as read by servo
1644
1645 OUTPUTS:
1646 -- voltage -- int -- volts
1647 THEORY OF OPERATION:
1648 -- mem_read the present_voltage register and convert
1649 """
1650 return self.dynamixel2voltage(self.mem_read(self.mcu.present_voltage))
1651
1684
1686 """
1687 Reset rotation count for the extended position read by get_xpos()
1688 """
1689 self.lastPos = 0
1690 self.xrot = 0
1691
1693 """
1694 Get extended position
1695
1696 Returns position as floating point in units of rotation.
1697 This can count an indefinite number of rotations, and may be
1698 used in Motor mode to get position
1699 """
1700 pos = float(self.get_pos()-self.MIN_ANG)/self.MAX_ANG
1701 if abs(pos-self.lastPos)>0.5:
1702 if pos<self.lastPos:
1703 self.xrot += 1
1704 else:
1705 self.xrot -= 1
1706 self.lastPos = pos
1707 return self.xrot + pos
1708
1745
1777
1779 """
1780 DESCRIPTION:
1781 -- AX12 Specific constants
1782 RESPONSIBILITIES:
1783 -- ...
1784 """
1785
1786 MAX_POS = 0x3FF
1787 MIN_POS = 0
1788 MIN_ANG = 0
1789 MAX_ANG = 30000
1790
1791
1792 MAX_TORQUE = 0x3FF
1793 DIRECTION_BIT = 1<<10
1794 TORQUE_SCL = float(MAX_TORQUE/1.0)
1795
1796
1797 SPEED_SCL = 0.114
1798
1799 VOLTAGE_SCL = 0.1
1800
1801 SCL = float(MAX_POS - MIN_POS)/(MAX_ANG - MIN_ANG)
1802 OFS = (MAX_POS - MIN_POS)/2 + MIN_POS
1803 - def __init__( self, node_id, typecode, pna ):
1804 """
1805 """
1806 DynamixelModule.__init__( self, node_id, typecode, pna )
1807
1838
1840 TYPECODE = "Dynamixel-FAKE"
1841
1844
1847
1850
1853
1854 MODELS = {
1855 'Dynamixel-006b' : (EX106MemWithOps,EX106Module),
1856 'Dynamixel-0040' : (RX64MemWithOps,RX64Module),
1857 'Dynamixel-0200' : (RX64MemWithOps,RX64Module),
1858 'Dynamixel-000c' : (AX12MemWithOps,AX12Module),
1859 'Dynamixel-001d' : (MX28MemWithOps,MX28Module),
1860 'Dynamixel-0136' : (MX64MemWithOps,MX64Module),
1861 MissingDynamixel.TYPECODE : (None,MissingDynamixel),
1862 }
1863