arbotix.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 # Copyright (c) 2008-2011 Vanadium Labs LLC.
4 # All right reserved.
5 #
6 # Redistribution and use in source and binary forms, with or without
7 # modification, are permitted provided that the following conditions are met:
8 #
9 # * Redistributions of source code must retain the above copyright
10 # notice, this list of conditions and the following disclaimer.
11 # * Redistributions in binary form must reproduce the above copyright
12 # notice, this list of conditions and the following disclaimer in the
13 # documentation and/or other materials provided with the distribution.
14 # * Neither the name of Vanadium Labs LLC nor the names of its
15 # contributors may be used to endorse or promote products derived
16 # from this software without specific prior written permission.
17 #
18 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
19 # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
20 # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
21 # DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
22 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
23 # LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
24 # OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
25 # LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
26 # OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
27 # ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28 
29 # Author: Michael Ferguson
30 
31 ## @file arbotix.py Low-level code to control an ArbotiX.
32 
33 import serial, time, sys, thread
34 from ax12 import *
35 from struct import unpack
36 
37 ## @brief ArbotiX errors. Used by now to handle broken serial connections.
38 class ArbotiXException(Exception):
39  pass
40 
41 ## @brief This class controls an ArbotiX, USBDynamixel, or similar board through a serial connection.
42 class ArbotiX:
43 
44  ## @brief Constructs an ArbotiX instance, optionally opening the serial connection.
45 
54  def __init__(self, port="/dev/ttyUSB0", baud=115200, timeout=0.1, open_port=True):
55  self._mutex = thread.allocate_lock()
56  self._ser = serial.Serial()
57 
58  self._ser.port = port
59  self._ser.baudrate = baud
60  self._ser.timeout = timeout
61 
62  if open_port:
63  self._ser.open()
64 
65  ## The last error level read back
66  self.error = 0
67 
68  def __write__(self, msg):
69  try:
70  self._ser.write(msg)
71  except serial.SerialException as e:
72  self._mutex.release()
73  raise ArbotiXException(e)
74 
75  def openPort(self):
76  self._ser.close()
77  try:
78  self._ser.open()
79  except serial.SerialException as e:
80  raise ArbotiXException(e)
81 
82  def closePort(self):
83  self._ser.close()
84 
85  ## @brief Read a dynamixel return packet in an iterative attempt.
86 
90  def getPacket(self, mode, id=-1, leng=-1, error=-1, params = None):
91  try:
92  d = self._ser.read()
93  except Exception as e:
94  print e
95  return None
96  # need a positive byte
97  if d == '':
98  return None
99 
100  # now process our byte
101  if mode == 0: # get our first 0xFF
102  if ord(d) == 0xff:
103  return self.getPacket(1)
104  else:
105  return self.getPacket(0)
106  elif mode == 1: # get our second 0xFF
107  if ord(d) == 0xff:
108  return self.getPacket(2)
109  else:
110  return self.getPacket(0)
111  elif mode == 2: # get id
112  if d != 0xff:
113  return self.getPacket(3, ord(d))
114  else:
115  return self.getPacket(0)
116  elif mode == 3: # get length
117  return self.getPacket(4, id, ord(d))
118  elif mode == 4: # read error
119  self.error = ord(d)
120  if leng == 2:
121  return self.getPacket(6, id, leng, ord(d), list())
122  else:
123  return self.getPacket(5, id, leng, ord(d), list())
124  elif mode == 5: # read params
125  params.append(ord(d))
126  if len(params) + 2 == leng:
127  return self.getPacket(6, id, leng, error, params)
128  else:
129  return self.getPacket(5, id, leng, error, params)
130  elif mode == 6: # read checksum
131  checksum = id + leng + error + sum(params) + ord(d)
132  if checksum % 256 != 255:
133  return None
134  return params
135  # fail
136  return None
137 
138  ## @brief Send an instruction to the device.
139 
149  def execute(self, index, ins, params, ret=True):
150  values = None
151  self._mutex.acquire()
152  try:
153  self._ser.flushInput()
154  except Exception as e:
155  print e
156  length = 2 + len(params)
157  checksum = 255 - ((index + length + ins + sum(params))%256)
158  self.__write__(chr(0xFF)+chr(0xFF)+chr(index)+chr(length)+chr(ins))
159  for val in params:
160  self.__write__(chr(val))
161  self.__write__(chr(checksum))
162  if ret:
163  values = self.getPacket(0)
164  self._mutex.release()
165  return values
166 
167  ## @brief Read values of registers.
168 
176  def read(self, index, start, length):
177  values = self.execute(index, AX_READ_DATA, [start, length])
178  if values == None:
179  return -1
180  else:
181  return values
182 
183  ## @brief Write values to registers.
184 
192  def write(self, index, start, values):
193  self.execute(index, AX_WRITE_DATA, [start] + values)
194  return self.error
195 
196  ## @brief Write values to registers on many servos.
197 
202  def syncWrite(self, start, values):
203  output = list()
204  for i in values:
205  output = output + i
206  length = len(output) + 4 # length of overall packet
207  lbytes = len(values[0])-1 # length of bytes to write to a servo
208  self._mutex.acquire()
209  try:
210  self._ser.flushInput()
211  except:
212  pass
213  self.__write__(chr(0xFF)+chr(0xFF)+chr(254)+chr(length)+chr(AX_SYNC_WRITE))
214  self.__write__(chr(start)) # start address
215  self.__write__(chr(lbytes)) # bytes to write each servo
216  for i in output:
217  self.__write__(chr(i))
218  checksum = 255 - ((254 + length + AX_SYNC_WRITE + start + lbytes + sum(output))%256)
219  self.__write__(chr(checksum))
220  self._mutex.release()
221 
222  ## @brief Read values of registers on many servos.
223 
231  def syncRead(self, servos, start, length):
232  return self.execute(0xFE, AX_SYNC_READ, [start, length] + servos )
233 
234  ## @brief Set baud rate of a device.
235 
241  def setBaud(self, index, baud):
242  return self.write(index, P_BAUD_RATE, [baud, ])
243 
244  ## @brief Get the return level of a device.
245 
249  def getReturnLevel(self, index):
250  try:
251  return int(self.read(index, P_RETURN_LEVEL, 1)[0])
252  except:
253  return -1
254 
255  ## @brief Set the return level of a device.
256 
262  def setReturnLevel(self, index, value):
263  return self.write(index, P_RETURN_LEVEL, [value])
264 
265  ## @brief Turn on the torque of a servo.
266 
270  def enableTorque(self, index):
271  return self.write(index, P_TORQUE_ENABLE, [1])
272 
273  ## @brief Turn on the torque of a servo.
274 
278  def disableTorque(self, index):
279  return self.write(index, P_TORQUE_ENABLE, [0])
280 
281  ## @brief Set the status of the LED on a servo.
282 
288  def setLed(self, index, value):
289  return self.write(index, P_LED, [value])
290 
291  ## @brief Set the position of a servo.
292 
298  def setPosition(self, index, value):
299  return self.write(index, P_GOAL_POSITION_L, [value%256, value>>8])
300 
301  ## @brief Set the speed of a servo.
302 
308  def setSpeed(self, index, value):
309  return self.write(index, P_GOAL_SPEED_L, [value%256, value>>8])
310 
311  ## @brief Get the position of a servo.
312 
316  def getPosition(self, index):
317  values = self.read(index, P_PRESENT_POSITION_L, 2)
318  try:
319  return int(values[0]) + (int(values[1])<<8)
320  except:
321  return -1
322 
323  ## @brief Get the speed of a servo.
324 
328  def getSpeed(self, index):
329  values = self.read(index, P_PRESENT_SPEED_L, 2)
330  try:
331  return int(values[0]) + (int(values[1])<<8)
332  except:
333  return -1
334 
335  ## @brief Get the goal speed of a servo.
336 
340  def getGoalSpeed(self, index):
341  values = self.read(index, P_GOAL_SPEED_L, 2)
342  try:
343  return int(values[0]) + (int(values[1])<<8)
344  except:
345  return -1
346 
347  ## @brief Get the voltage of a device.
348 
352  def getVoltage(self, index):
353  try:
354  return int(self.read(index, P_PRESENT_VOLTAGE, 1)[0])/10.0
355  except:
356  return -1
357 
358  ## @brief Get the temperature of a device.
359 
363  def getTemperature(self, index):
364  try:
365  return int(self.read(index, P_PRESENT_TEMPERATURE, 1)[0])
366  except:
367  return -1
368 
369  ## @brief Determine if a device is moving.
370 
374  def isMoving(self, index):
375  try:
376  d = self.read(index, P_MOVING, 1)[0]
377  except:
378  return True
379  return d != 0
380 
381  ## @brief Put a servo into wheel mode (continuous rotation).
382 
384  def enableWheelMode(self, index):
385  self.write(index, P_CCW_ANGLE_LIMIT_L, [0,0])
386 
387  ## @brief Put a servo into servo mode.
388 
395  def disableWheelMode(self, index, resolution=10):
396  resolution = (2 ** resolution) - 1
397  self.write(index, P_CCW_ANGLE_LIMIT_L, [resolution%256,resolution>>8])
398 
399  ## Direction definition for setWheelSpeed
400  FORWARD = 0
401  ## Direction definition for setWheelSpeed
402  BACKWARD = 1
403 
404  ## @brief Set the speed and direction of a servo which is in wheel mode (continuous rotation).
405 
413  def setWheelSpeed(self, index, direction, speed):
414  if speed > 1023:
415  speed = 1023
416  if direction == self.FORWARD:
417  # 0~1023 is forward, it is stopped by setting to 0 while rotating to CCW direction.
418  self.write(index, P_GOAL_SPEED_L, [speed%256, speed>>8])
419  else:
420  # 1024~2047 is backward, it is stopped by setting to 1024 while rotating to CW direction.
421  speed += 1024
422  self.write(index, P_GOAL_SPEED_L, [speed%256, speed>>8])
423 
424  ###########################################################################
425  # Extended ArbotiX Driver
426 
427  ## Helper definition for analog and digital access.
428  LOW = 0
429  ## Helper definition for analog and digital access.
430  HIGH = 0xff
431  ## Helper definition for analog and digital access.
432  INPUT = 0
433  ## Helper definition for analog and digital access.
434  OUTPUT = 0xff
435 
436  # ArbotiX-specific register table
437  # We do Model, Version, ID, Baud, just like the AX-12
438  ## Register base address for reading digital ports
439  REG_DIGITAL_IN0 = 5
440  REG_DIGITAL_IN1 = 6
441  REG_DIGITAL_IN2 = 7
442  REG_DIGITAL_IN3 = 8
443  ## Register address for triggering rescan
444  REG_RESCAN = 15
445  # 16, 17 = RETURN, ALARM
446  ## Register address of first analog port (read only).
447  ## Each additional port is BASE + index.
448  ANA_BASE = 18
449  ## Register address of analog servos. Up to 10 servos, each
450  ## uses 2 bytes (L, then H), pulse width (0, 1000-2000ms) (Write only)
451  SERVO_BASE = 26
452  # Address 46 is Moving, just like an AX-12
453  REG_DIGITAL_OUT0 = 47
454 
455  ## @brief Force the ArbotiX2 to rescan the Dynamixel busses.
456  def rescan(self):
457  self.write(253, self.REG_RESCAN, [1,])
458 
459  ## @brief Get the value of an analog input pin.
460 
466  def getAnalog(self, index, leng=1):
467  try:
468  val = self.read(253, self.ANA_BASE+int(index), leng)
469  return sum(val[i] << (i * 8) for i in range(leng))
470  except:
471  return -1
472 
473  ## @brief Get the value of an digital input pin.
474 
478  def getDigital(self, index):
479  try:
480  if index < 32:
481  x = self.read(253, self.REG_DIGITAL_IN0 + int(index/8), 1)[0]
482  else:
483  return -1
484  except:
485  return -1
486  if x & (2**(index%8)):
487  return 255
488  else:
489  return 0
490 
491  ## @brief Get the value of an digital input pin.
492 
500  def setDigital(self, index, value, direction=0xff):
501  if index > 31: return -1
502  if value == 0 and direction > 0:
503  self.write(253, self.REG_DIGITAL_OUT0 + int(index), [1])
504  elif value > 0 and direction > 0:
505  self.write(253, self.REG_DIGITAL_OUT0 + int(index), [3])
506  elif value > 0 and direction == 0:
507  self.write(253, self.REG_DIGITAL_OUT0 + int(index), [2])
508  else:
509  self.write(253, self.REG_DIGITAL_OUT0 + int(index), [0])
510  return 0
511 
512  ## @brief Set the position of a hobby servo.
513 
520  def setServo(self, index, value):
521  if index > 7: return -1
522  if value != 0 and (value < 500 or value > 2500):
523  print "ArbotiX Error: Servo value out of range:", value
524  else:
525  self.write(253, self._SERVO_BASE + 2*index, [value%256, value>>8])
526  return 0
527 
def rescan(self)
Force the ArbotiX2 to rescan the Dynamixel busses.
Definition: arbotix.py:456
def getPacket(self, mode, id=-1, leng=-1, error=-1, params=None)
Read a dynamixel return packet in an iterative attempt.
Definition: arbotix.py:90
def enableWheelMode(self, index)
Put a servo into wheel mode (continuous rotation).
Definition: arbotix.py:384
def disableTorque(self, index)
Turn on the torque of a servo.
Definition: arbotix.py:278
def getReturnLevel(self, index)
Get the return level of a device.
Definition: arbotix.py:249
int REG_RESCAN
Register address for triggering rescan.
Definition: arbotix.py:444
def setDigital(self, index, value, direction=0xff)
Get the value of an digital input pin.
Definition: arbotix.py:500
def setPosition(self, index, value)
Set the position of a servo.
Definition: arbotix.py:298
def __init__(self, port="/dev/ttyUSB0", baud=115200, timeout=0.1, open_port=True)
Constructs an ArbotiX instance, optionally opening the serial connection.
Definition: arbotix.py:54
def execute(self, index, ins, params, ret=True)
Send an instruction to the device.
Definition: arbotix.py:149
def setReturnLevel(self, index, value)
Set the return level of a device.
Definition: arbotix.py:262
def write(self, index, start, values)
Write values to registers.
Definition: arbotix.py:192
def setBaud(self, index, baud)
Set baud rate of a device.
Definition: arbotix.py:241
def syncRead(self, servos, start, length)
Read values of registers on many servos.
Definition: arbotix.py:231
def getSpeed(self, index)
Get the speed of a servo.
Definition: arbotix.py:328
def getVoltage(self, index)
Get the voltage of a device.
Definition: arbotix.py:352
def disableWheelMode(self, index, resolution=10)
Put a servo into servo mode.
Definition: arbotix.py:395
def getAnalog(self, index, leng=1)
Get the value of an analog input pin.
Definition: arbotix.py:466
def __write__(self, msg)
Definition: arbotix.py:68
def setServo(self, index, value)
Set the position of a hobby servo.
Definition: arbotix.py:520
int REG_DIGITAL_IN0
Register base address for reading digital ports.
Definition: arbotix.py:439
def getTemperature(self, index)
Get the temperature of a device.
Definition: arbotix.py:363
int FORWARD
Direction definition for setWheelSpeed.
Definition: arbotix.py:400
This class controls an ArbotiX, USBDynamixel, or similar board through a serial connection.
Definition: arbotix.py:42
def setLed(self, index, value)
Set the status of the LED on a servo.
Definition: arbotix.py:288
def enableTorque(self, index)
Turn on the torque of a servo.
Definition: arbotix.py:270
def setSpeed(self, index, value)
Set the speed of a servo.
Definition: arbotix.py:308
def read(self, index, start, length)
Read values of registers.
Definition: arbotix.py:176
def getPosition(self, index)
Get the position of a servo.
Definition: arbotix.py:316
def getDigital(self, index)
Get the value of an digital input pin.
Definition: arbotix.py:478
def setWheelSpeed(self, index, direction, speed)
Set the speed and direction of a servo which is in wheel mode (continuous rotation).
Definition: arbotix.py:413
def isMoving(self, index)
Determine if a device is moving.
Definition: arbotix.py:374
def syncWrite(self, start, values)
Write values to registers on many servos.
Definition: arbotix.py:202
int ANA_BASE
Register address of first analog port (read only).
Definition: arbotix.py:448
def getGoalSpeed(self, index)
Get the goal speed of a servo.
Definition: arbotix.py:340
error
The last error level read back.
Definition: arbotix.py:66


arbotix_python
Author(s): Michael Ferguson
autogenerated on Fri Jun 7 2019 21:54:10