1
2
3
4
5
6
7 """
8 This code was written to work with the Pololu Qik motor controllers.
9
10 by Carl J. Nobile
11
12 THE SOFTWARE IS PROVIDED 'AS IS', WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
13 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
14 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
15 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
16 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
17 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
18 SOFTWARE.
19 """
20 __docformat__ = "reStructuredText en"
21
22
23 import serial
24
25 from .crc7 import crc7
26
27
29 _BAUD_DETECT = 0xAA
30 _CONFIG_RETURN = {
31 0: 'OK',
32 1: 'Invalid Parameter',
33 2: 'Invalid Value',
34 }
35 _BOOL_TO_INT = {False: 0, True: 1}
36
37 - def __init__(self, device, baud, readTimeout, writeTimeout, log):
38 self._log = log
39 self._device_numbers = []
40 self._serial = serial.Serial(port=device, baudrate=baud,
41 bytesize=serial.EIGHTBITS,
42 parity=serial.PARITY_NONE,
43 stopbits=serial.STOPBITS_ONE,
44 timeout=readTimeout,
45 writeTimeout=writeTimeout)
46 self.setPololuProtocol()
47 self._timeoutToValue = self._genTimeoutList(self.DEFAULT_SERIAL_TIMEOUT)
48 self._valueToTimeout = dict(
49 [(v, k) for k, v in self._timeoutToValue.items()])
50 self._timeoutKeys = self._timeoutToValue.keys()
51 self._timeoutKeys.sort()
52 self._deviceConfig = {}
53 self._crc = False
54
56 """
57 Generates a dict of the valid timeout values for the given `const`.
58 The `const` value may change for different versions or types of
59 Pololu boards.
60 """
61 result = {}
62
63 for v in range(128):
64 x = v & 0x0F
65 y = (v >> 4) & 0x07
66
67 if not y or (y and x > 7):
68 result[const * x * 2**y] = v
69
70 self._log and self._log.debug("Timeout list: %s", result)
71 return result
72
74 """
75 Find all the devices on the serial buss and store the results in a
76 class member object.
77 """
78 tmpTimeout = self._serial.timeout
79 self._serial.timeout = 0.01
80
81 for dev in range(128):
82 device = self._getDeviceID(dev)
83
84 if device is not None and int(device) not in self._deviceConfig:
85 config = self._deviceConfig.setdefault(int(device), {})
86 self._deviceCallback(device, config)
87 self._log and self._log.info(
88 "Found device '%s' with configuration: %s", device, config)
89
90 self._serial.timeout = tmpTimeout
91
93 raise NotImplementedError("Need to implement _deviceCallback.")
94
96 """
97 Get a dictionary of the current hardware configuration options for
98 the device.
99
100 :Returns:
101 Dictionary of current configuration options.
102 """
103 return self._deviceConfig.get(device, {})
104
106 """
107 Closes the serial connection.
108 """
109 if self._serial:
110 self._serial.close()
111
113 """
114 Check if the serial connection is open.
115
116 :Returns:
117 If `True` the serial connction is open else if `False` it is closed.
118 """
119 return self._serial.isOpen()
120
122 """
123 Set the compact protocol.
124 """
125 self._compact = True
126 self._serial.write(bytes(self._BAUD_DETECT))
127 self._log and self._log.debug("Compact protocol has been set.")
128
130 """
131 Check if currently using the compact protocol.
132
133 :Returns:
134 if `True` the compact protocol is currently being used else if
135 `False` it is not currently being used.
136 """
137 return self._compact == True
138
140 """
141 Set the pololu protocol.
142 """
143 self._compact = False
144 self._log and self._log.debug("Pololu protocol has been set.")
145
147 """
148 Check if currently using the pololu protocol.
149
150 :Returns:
151 if `True` the pololu protocol is currently being used else if
152 `False` it is not currently being used.
153 """
154 return self._compact == False
155
157 """
158 Enable or disable cyclic redundancy check.
159
160 :Parameters:
161 value : `bool`
162 If `True` CRC is enabled else if `False` CRC is disabled.
163 """
164 self._crc = value
165
167 """
168 Check if CRC is enabled.
169
170 :Returns:
171 If `True` CRC is enabled else if `False` CRC is disabled.
172 """
173 return self._crc == True
174
175 - def _writeData(self, command, device, params=()):
176 """
177 Write the data to the device.
178
179 :Parameters:
180 command : `int`
181 The command to write to the device.
182 device : `int`
183 The device is the integer number of the hardware devices ID and
184 is only used with the Pololu Protocol.
185 params : `tuple`
186 Sequence of bytes to write.
187
188 :Exceptions:
189 * `SerialTimeoutException`
190 If the low level serial package times out.
191 * `SerialException`
192 IO error when the port is not open.
193 """
194 sequence = []
195
196 if self._compact:
197 sequence.append(command | 0x80)
198 else:
199 sequence.append(self._BAUD_DETECT)
200 sequence.append(device)
201 sequence.append(command)
202
203 for param in params:
204 sequence.append(param)
205
206 if self._crc:
207 sequence.append(crc7(sequence))
208
209 self._serial.write(bytearray(sequence))
210 self._log and self._log.debug("Wrote byte sequence: %s",
211 [hex(num) for num in sequence])
212
214 """
215 Get the firmware version.
216
217 :Parameters:
218 device : `int`
219 The device is the integer number of the hardware devices ID and
220 is only used with the Pololu Protocol.
221
222 :Returns:
223 An integer indicating the version number.
224 """
225 cmd = self._COMMAND.get('get-fw-version')
226 self._writeData(cmd, device)
227
228 try:
229 result = self._serial.read(size=1)
230 result = int(result)
231 except serial.SerialException as e:
232 self._log and self._log.error("Error: %s", e, exc_info=True)
233 raise e
234 except ValueError as e:
235 result = None
236
237 return result
238
240 """
241 Get the error message or value stored in the Qik hardware.
242
243 :Parameters:
244 device : `int`
245 The device is the integer number of the hardware devices ID and
246 is only used with the Pololu Protocol.
247 message : `bool`
248 If set to `True` a text message will be returned, if set to `False`
249 the integer stored in the Qik will be returned.
250
251 :Returns:
252 A list of text messages, integers, or and empty list. See the
253 `message` parameter above.
254 """
255 cmd = self._COMMAND.get('get-error')
256 self._writeData(cmd, device)
257 result = []
258 bits = []
259
260 try:
261 num = self._serial.read(size=1)
262 num = ord(num)
263 except serial.SerialException as e:
264 self._log and self._log.error("Error: %s", e, exc_info=True)
265 raise e
266 except TypeError as e:
267 num = 0
268
269 for i in range(7, -1, -1):
270 bit = num & (1 << i)
271
272 if bit:
273 if message:
274 result.append(self._ERRORS.get(bit))
275 else:
276 result.append(bit)
277
278 return result
279
281 """
282 Low level method used for all get config commands.
283
284 :Parameters:
285 num : `int`
286 Number that indicates the config option to get from the hardware.
287 device : `int`
288 The device is the integer number of the hardware devices ID and
289 is only used with the Pololu Protocol.
290
291 :Returns:
292 An integer representing the value stored in the hardware device.
293 """
294 cmd = self._COMMAND.get('get-config')
295 self._writeData(cmd, device, params=(num,))
296
297 try:
298 result = self._serial.read(size=1)
299 result = ord(result)
300 except serial.SerialException as e:
301 self._log and self._log.error("Error: %s", e, exc_info=True)
302 raise e
303 except TypeError as e:
304 result = None
305
306 return result
307
309 """
310 Get the device ID.
311
312 :Parameters:
313 device : `int`
314 The device is the integer number of the hardware devices ID and
315 is only used with the Pololu Protocol.
316
317 :Returns:
318 An integer number of the hardware device ID.
319 """
320 return self._getConfig(self.DEVICE_ID, device)
321
323 """
324 Get the PWM frequency stored on the hardware device.
325
326 :Parameters:
327 device : `int`
328 The device is the integer number of the hardware devices ID and
329 is only used with the Pololu Protocol.
330 message : `bool`
331 If set to `True` a text message will be returned, if set to `False`
332 the integer stored in the Qik will be returned.
333
334 :Returns:
335 A text message or an int. See the `message` parameter above.
336 """
337 result = self._getConfig(self.PWM_PARAM, device)
338 freq, msg = self._CONFIG_PWM.get(result, (result, 'Invalid Frequency'))
339
340 if message:
341 result = msg
342 else:
343 result = freq
344
345 return result
346
348 """
349 Get the motor shutdown on error status stored on the hardware device.
350
351 :Parameters:
352 device : `int`
353 The device is the integer number of the hardware devices ID and
354 is only used with the Pololu Protocol.
355
356 :Returns:
357 Returns `True` when morot will shutdown on and error, else `False`.
358 """
359 return bool(self._getConfig(self.MOTOR_ERR_SHUTDOWN, device))
360
362 """
363 Get the serial timeout stored on the hardware device.
364
365 Caution, more that one value returned from the Qik can have the same
366 actual timeout value according the the formula below. I have verified
367 this as an idiosyncrasy of the Qik itself. There are only a total of
368 72 unique values that the Qik can logically use the remaining 56
369 values are repeats of the 72.
370
371 :Parameters:
372 device : `int`
373 The device is the integer number of the hardware devices ID and
374 is only used with the Pololu Protocol.
375
376 :Returns:
377 The timeout value in seconds.
378 """
379 num = self._getConfig(self.SERIAL_TIMEOUT, device)
380
381 if isinstance(num, int):
382 x = num & 0x0F
383 y = (num >> 4) & 0x07
384 result = self.DEFAULT_SERIAL_TIMEOUT * x * pow(2, y)
385 else:
386 result = num
387
388 return result
389
390 - def _setConfig(self, num, value, device, message):
391 """
392 Low level method used for all set config commands.
393
394 :Parameters:
395 num : `int`
396 Number that indicates the config option to get from the hardware.
397 value : `int`
398 The value to set in the hardware device.
399 device : `int`
400 The device is the integer number of the hardware devices ID and
401 is only used with the Pololu Protocol.
402 message : `bool`
403 If set to `True` a text message will be returned, if set to `False`
404 the integer stored in the Qik will be returned.
405
406 :Returns:
407 A text message or an int. See the `message` parameter above.
408
409 :Exceptions:
410 * `SerialException`
411 IO error indicating there was a problem reading from the serial
412 connection.
413 """
414 cmd = self._COMMAND.get('set-config')
415 self._writeData(cmd, device, params=(num, value, 0x55, 0x2A))
416
417 try:
418 result = self._serial.read(size=1)
419 result = ord(result)
420 except serial.SerialException as e:
421 self._log and self._log.error("Error: %s", e, exc_info=True)
422 raise e
423 except TypeError as e:
424 result = None
425
426 if result is not None and message:
427 result = self._CONFIG_RETURN.get(
428 result, 'Unknown return value: {}'.format(result))
429
430 return result
431
433 """
434 Set the hardware device number. This is only needed if more that one
435 device is on the same serial buss.
436
437 :Parameters:
438 value : `int`
439 The device ID to set in the range of 0 - 127.
440 device : `int`
441 The device is the integer number of the hardware devices ID and
442 is only used with the Pololu Protocol.
443 message : `bool`
444 If set to `True` a text message will be returned, if set to `False`
445 the integer stored in the Qik will be returned.
446
447 :Returns:
448 A text message or an int. See the `message` parameter above. If
449 `value` and `device` are the same `OK` or `0` will be returned
450 depending on the value of `message`.
451 """
452 if value != device:
453 result = self._setConfig(self.DEVICE_ID, value, device, message)
454 self._deviceConfig[value] = self._deviceConfig.pop(device)
455 elif message:
456 result = self._CONFIG_RETURN.get(0)
457 elif not message:
458 result = 0
459
460 return result
461
463 """
464 Set the PWM frequency.
465
466 :Parameters:
467 pwm : `int`
468 The PWN frequency to set in hertz.
469 device : `int`
470 The device is the integer number of the hardware devices ID and
471 is only used with the Pololu Protocol.
472 message : `bool`
473 If set to `True` a text message will be returned, if set to `False`
474 the integer stored in the Qik will be returned.
475
476 :Returns:
477 A text message or an int. See the `message` parameter above.
478 """
479 value = self._CONFIG_PWM_TO_VALUE.get(pwm)
480
481 if value is None:
482 msg = "Invalid frequency: {}".format(pwm)
483 self._log and self._log.error(msg)
484 raise ValueError(msg)
485
486 self._deviceConfig[device]['pwm'] = value
487 return self._setConfig(self.PWM_PARAM, value, device, message)
488
490 """
491 Set the motor shutdown on error status stored on the hardware device.
492
493 :Parameters:
494 value : `int`
495 An integer indicating the effect on the motors when an error occurs.
496 device : `int`
497 The device is the integer number of the hardware devices ID and
498 is only used with the Pololu Protocol.
499 message : `bool`
500 If set to `True` a text message will be returned, if set to `False`
501 the integer stored in the Qik will be returned.
502
503 :Returns:
504 Text message indicating the status of the shutdown error.
505 """
506 value = self._BOOL_TO_INT.get(value, 1)
507 self._deviceConfig[device]['shutdown'] = value
508 return self._setConfig(self.MOTOR_ERR_SHUTDOWN, value, device, message)
509
511 """
512 Set the serial timeout on the hardware device.
513
514 :Parameters:
515 timeout : `float` or `int`
516 The timeout value as defined by the hardware manual.
517 device : `int`
518 The device is the integer number of the hardware devices ID and
519 is only used with the Pololu Protocol.
520 message : `bool`
521 If set to `True` a text message will be returned, if set to `False`
522 the integer stored in the Qik will be returned.
523
524 :Returns:
525 Text message indicating the status of the shutdown error.
526 """
527 timeout = min(self._timeoutKeys, key=lambda x: abs(x-timeout))
528 value = self._timeoutToValue.get(timeout, 0)
529 self._deviceConfig[device]['timeout'] = timeout
530 return self._setConfig(self.SERIAL_TIMEOUT, value, device, message)
531
533 """
534 Set motor 0 speed.
535
536 :Parameters:
537 speed : `int`
538 Motor speed as an integer.
539 device : `int`
540 The device is the integer number of the hardware devices ID and
541 is only used with the Pololu Protocol.
542 """
543 self._setSpeed(speed, 'm0', device)
544
546 """
547 Set motor 1 speed.
548
549 :Parameters:
550 speed : `int`
551 Motor speed as an integer.
552 device : `int`
553 The device is the integer number of the hardware devices ID and
554 is only used with the Pololu Protocol.
555 """
556 self._setSpeed(speed, 'm1', device)
557
559 """
560 Set motor speed. This method takes into consideration the PWM frequency
561 that the hardware is currently running at and limits the values passed
562 to the hardware accordingly.
563
564 :Parameters:
565 speed : `int`
566 Motor speed as an integer. Negative numbers indicate reverse
567 speeds.
568 motor : `str`
569 A string value indicating the motor to set the speed on.
570 device : `int`
571 The device is the integer number of the hardware devices ID and
572 is only used with the Pololu Protocol.
573 """
574 reverse = False
575
576 if speed < 0:
577 speed = -speed
578 reverse = True
579
580
581 if self._deviceConfig[device]['pwm'] in (0, 2, 4,) and speed > 127:
582 speed = 127
583
584 if speed > 127:
585 if speed > 255:
586 speed = 255
587
588 if reverse:
589 cmd = self._COMMAND.get('{}-reverse-8bit'.format(motor))
590 else:
591 cmd = self._COMMAND.get('{}-forward-8bit'.format(motor))
592
593 speed -= 128
594 else:
595 if reverse:
596 cmd = self._COMMAND.get('{}-reverse-7bit'.format(motor))
597 else:
598 cmd = self._COMMAND.get('{}-forward-7bit'.format(motor))
599
600 if not cmd:
601 msg = "Invalid motor specified: {}".format(motor)
602 self._log and self._log.error(msg)
603 raise ValueError(msg)
604
605 self._writeData(cmd, device, params=(speed,))
606