arbotix.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # Copyright (c) 2008-2011 Vanadium Labs LLC. 
00004 # All right reserved.
00005 #
00006 # Redistribution and use in source and binary forms, with or without
00007 # modification, are permitted provided that the following conditions are met:
00008 #
00009 #   * Redistributions of source code must retain the above copyright
00010 #     notice, this list of conditions and the following disclaimer.
00011 #   * Redistributions in binary form must reproduce the above copyright
00012 #     notice, this list of conditions and the following disclaimer in the
00013 #     documentation and/or other materials provided with the distribution.
00014 #   * Neither the name of Vanadium Labs LLC nor the names of its 
00015 #     contributors may be used to endorse or promote products derived 
00016 #     from this software without specific prior written permission.
00017 #
00018 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00019 # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00020 # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00021 # DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
00022 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
00023 # LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
00024 # OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
00025 # LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
00026 # OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
00027 # ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00028 
00029 # Author: Michael Ferguson
00030 
00031 ## @file arbotix.py Low-level code to control an ArbotiX.
00032 
00033 import serial, time, sys, thread
00034 from ax12 import *
00035 from struct import unpack
00036 
00037 ## @brief This class controls an ArbotiX, USBDynamixel, or similar board through a serial connection.
00038 class ArbotiX:
00039 
00040     ## @brief Constructs an ArbotiX instance and opens the serial connection.
00041     ##
00042     ## @param port The name of the serial port to open.
00043     ## 
00044     ## @param baud The baud rate to run the port at. 
00045     ##
00046     ## @param timeout The timeout to use for the port. When operating over a wireless link, you may need to
00047     ## increase this.
00048     def __init__(self, port="/dev/ttyUSB0",baud=115200, timeout = 0.1):
00049         self._mutex = thread.allocate_lock()
00050         self._ser = serial.Serial()
00051         
00052         self._ser.baudrate = baud
00053         self._ser.port = port
00054         self._ser.timeout = timeout
00055         self._ser.open()
00056 
00057         ## The last error level read back
00058         self.error = 0
00059 
00060     ## @brief Read a dynamixel return packet in an iterative attempt.
00061     ##
00062     ## @param mode This should be 0 to start reading packet. 
00063     ##
00064     ## @return The error level returned by the device. 
00065     def getPacket(self, mode, id=-1, leng=-1, error=-1, params = None):
00066         try:
00067             d = self._ser.read()     
00068         except Exception as e:
00069             print e
00070             return None
00071         # need a positive byte
00072         if d == '':
00073             return None
00074 
00075         # now process our byte
00076         if mode == 0:           # get our first 0xFF
00077             if ord(d) == 0xff:   
00078                 return self.getPacket(1)
00079             else:
00080                 return self.getPacket(0)
00081         elif mode == 1:         # get our second 0xFF
00082             if ord(d) == 0xff:
00083                 return self.getPacket(2)
00084             else:
00085                 return self.getPacket(0)
00086         elif mode == 2:         # get id
00087             if d != 0xff:
00088                 return self.getPacket(3, ord(d))
00089             else:              
00090                 return self.getPacket(0)
00091         elif mode == 3:         # get length
00092             return self.getPacket(4, id, ord(d))
00093         elif mode == 4:         # read error    
00094             self.error = ord(d)
00095             if leng == 2:
00096                 return self.getPacket(6, id, leng, ord(d), list())
00097             else:
00098                 return self.getPacket(5, id, leng, ord(d), list())
00099         elif mode == 5:         # read params
00100             params.append(ord(d))
00101             if len(params) + 2 == leng:
00102                 return self.getPacket(6, id, leng, error, params)
00103             else:
00104                 return self.getPacket(5, id, leng, error, params)
00105         elif mode == 6:         # read checksum
00106             checksum = id + leng + error + sum(params) + ord(d)
00107             if checksum % 256 != 255:
00108                 return None
00109             return params
00110         # fail
00111         return None
00112 
00113     ## @brief Send an instruction to the device. 
00114     ##
00115     ## @param index The ID of the servo to write.
00116     ##
00117     ## @param ins The instruction to send.
00118     ##
00119     ## @param params A list of the params to send.
00120     ##
00121     ## @param ret Whether to read a return packet.
00122     ##
00123     ## @return The return packet, if read.
00124     def execute(self, index, ins, params, ret=True):
00125         values = None
00126         self._mutex.acquire()  
00127         try:      
00128             self._ser.flushInput()
00129         except Exception as e:
00130             print e
00131         length = 2 + len(params)
00132         checksum = 255 - ((index + length + ins + sum(params))%256)
00133         try: 
00134             self._ser.write(chr(0xFF)+chr(0xFF)+chr(index)+chr(length)+chr(ins))
00135         except Exception as e:
00136             print e
00137             self._mutex.release()
00138             return None
00139         for val in params:
00140             try:
00141                 self._ser.write(chr(val))
00142             except Exception as e:
00143                 print e
00144                 self._mutex.release()
00145                 return None
00146         try:
00147             self._ser.write(chr(checksum))
00148         except Exception as e:
00149             print e
00150             self._mutex.release()
00151             return None
00152         if ret:
00153             values = self.getPacket(0)
00154         self._mutex.release()
00155         return values
00156     
00157     ## @brief Read values of registers.
00158     ##
00159     ## @param index The ID of the servo.
00160     ## 
00161     ## @param start The starting register address to begin the read at.
00162     ##
00163     ## @param length The number of bytes to read.
00164     ##
00165     ## @return A list of the bytes read, or -1 if failure.
00166     def read(self, index, start, length):
00167         values = self.execute(index, AX_READ_DATA, [start, length])
00168         if values == None:
00169             return -1        
00170         else:
00171             return values
00172 
00173     ## @brief Write values to registers.
00174     ##
00175     ## @param index The ID of the servo.
00176     ##
00177     ## @param start The starting register address to begin writing to.
00178     ##
00179     ## @param values The data to write, in a list.
00180     ##
00181     ## @return The error level.
00182     def write(self, index, start, values):
00183         self.execute(index, AX_WRITE_DATA, [start] + values)
00184         return self.error     
00185 
00186     ## @brief Write values to registers on many servos.
00187     ##
00188     ## @param start The starting register address to begin writing to.
00189     ##
00190     ## @param values The data to write, in a list of lists. Format should be
00191     ## [(id1, val1, val2), (id2, val1, val2)]
00192     def syncWrite(self, start, values):
00193         output = list()
00194         for i in values:
00195             output = output + i 
00196         length = len(output) + 4                # length of overall packet
00197         lbytes = len(values[0])-1               # length of bytes to write to a servo               
00198         self._mutex.acquire()  
00199         try:      
00200             self._ser.flushInput()
00201         except:
00202             pass  
00203         self._ser.write(chr(0xFF)+chr(0xFF)+chr(254)+chr(length)+chr(AX_SYNC_WRITE))        
00204         self._ser.write(chr(start))              # start address
00205         self._ser.write(chr(lbytes))             # bytes to write each servo
00206         for i in output:
00207             self._ser.write(chr(i))
00208         checksum = 255 - ((254 + length + AX_SYNC_WRITE + start + lbytes + sum(output))%256)
00209         self._ser.write(chr(checksum))
00210         self._mutex.release()
00211 
00212     ## @brief Read values of registers on many servos.
00213     ##
00214     ## @param servos A list of the servo IDs to read from.
00215     ##
00216     ## @param start The starting register address to begin reading at.
00217     ##
00218     ## @param length The number of bytes to read from each servo.
00219     ##
00220     ## @return A list of bytes read.
00221     def syncRead(self, servos, start, length):
00222         return self.execute(0xFE, AX_SYNC_READ, [start, length] + servos )
00223     
00224     ## @brief Set baud rate of a device.
00225     ##
00226     ## @param index The ID of the device to write (Note: ArbotiX is 253).
00227     ##
00228     ## @param baud The baud rate.
00229     ##
00230     ## @return The error level.
00231     def setBaud(self, index, baud):
00232         return self.write(index, P_BAUD_RATE, [baud, ])
00233 
00234     ## @brief Get the return level of a device.
00235     ##
00236     ## @param index The ID of the device to read.
00237     ##
00238     ## @return The return level, .
00239     def getReturnLevel(self, index):
00240         try:
00241             return int(self.read(index, P_RETURN_LEVEL, 1)[0])
00242         except:
00243             return -1
00244 
00245     ## @brief Set the return level of a device.
00246     ##
00247     ## @param index The ID of the device to write.
00248     ##
00249     ## @param value The return level.
00250     ##
00251     ## @return The error level.
00252     def setReturnLevel(self, index, value):
00253         return self.write(index, P_RETURN_LEVEL, [value])        
00254 
00255     ## @brief Turn on the torque of a servo.
00256     ##
00257     ## @param index The ID of the device to enable.
00258     ##
00259     ## @return The error level.
00260     def enableTorque(self, index):
00261         return self.write(index, P_TORQUE_ENABLE, [1])
00262 
00263     ## @brief Turn on the torque of a servo.
00264     ##
00265     ## @param index The ID of the device to disable.
00266     ##
00267     ## @return The error level.
00268     def disableTorque(self, index):
00269         return self.write(index, P_TORQUE_ENABLE, [0])
00270 
00271     ## @brief Set the status of the LED on a servo.
00272     ##
00273     ## @param index The ID of the device to write.
00274     ##
00275     ## @param value 0 to turn the LED off, >0 to turn it on
00276     ##
00277     ## @return The error level.
00278     def setLed(self, index, value):
00279         return self.write(index, P_LED, [value])
00280 
00281     ## @brief Set the position of a servo.
00282     ##
00283     ## @param index The ID of the device to write.
00284     ##
00285     ## @param value The position to go to in, in servo ticks.
00286     ##
00287     ## @return The error level.
00288     def setPosition(self, index, value):
00289         return self.write(index, P_GOAL_POSITION_L, [value%256, value>>8])
00290 
00291     ## @brief Set the speed of a servo.
00292     ##
00293     ## @param index The ID of the device to write.
00294     ##
00295     ## @param value The speed to write.
00296     ##
00297     ## @return The error level.
00298     def setSpeed(self, index, value):
00299         return self.write(index, P_GOAL_SPEED_L, [value%256, value>>8])
00300 
00301     ## @brief Get the position of a servo.
00302     ##
00303     ## @param index The ID of the device to read.
00304     ##
00305     ## @return The servo position.
00306     def getPosition(self, index):
00307         values = self.read(index, P_PRESENT_POSITION_L, 2)
00308         try:
00309             return int(values[0]) + (int(values[1])<<8)
00310         except:
00311             return -1
00312 
00313     ## @brief Get the voltage of a device.
00314     ##
00315     ## @param index The ID of the device to read.
00316     ##
00317     ## @return The voltage, in Volts.
00318     def getVoltage(self, index):
00319         try:
00320             return int(self.read(index, P_PRESENT_VOLTAGE, 1)[0])/10.0
00321         except:
00322             return -1    
00323 
00324     ## @brief Get the temperature of a device.
00325     ##
00326     ## @param index The ID of the device to read.
00327     ##
00328     ## @return The temperature, in degrees C.
00329     def getTemperature(self, index):
00330         try:
00331             return int(self.read(index, P_PRESENT_TEMPERATURE, 1)[0])
00332         except:
00333             return -1
00334 
00335     ## @brief Determine if a device is moving.
00336     ##
00337     ## @param index The ID of the device to read.
00338     ##
00339     ## @return True if servo is moving.
00340     def isMoving(self, index):
00341         try:
00342             d = self.read(index, P_MOVING, 1)[0]
00343         except:
00344             return True
00345         return d != 0
00346     
00347     ## @brief Put a servo into wheel mode (continuous rotation).
00348     ##
00349     ## @param index The ID of the device to write.
00350     def enableWheelMode(self, index):
00351         self.write(index, P_CCW_ANGLE_LIMIT_L, [0,0])
00352 
00353     ## @brief Put a servo into servo mode.
00354     ##
00355     ## @param index The ID of the device to write.
00356     ##
00357     ## @param resolution The resolution of the encoder on the servo. NOTE: if using 
00358     ## 12-bit resolution servos (EX-106, MX-28, etc), you must pass resolution = 12.
00359     ##
00360     ## @return 
00361     def disableWheelMode(self, index, resolution=10):
00362         resolution = (2 ** resolution) - 1
00363         self.write(index, P_CCW_ANGLE_LIMIT_L, [resolution%256,resolution>>8])
00364 
00365     ## Direction definition for setWheelSpeed
00366     FORWARD = 0
00367     ## Direction definition for setWheelSpeed
00368     BACKWARD = 1
00369 
00370     ## @brief Set the speed and direction of a servo which is in wheel mode (continuous rotation).
00371     ##
00372     ## @param index The ID of the device to write.
00373     ##
00374     ## @param direction The direction of rotation, either FORWARD or BACKWARD
00375     ##
00376     ## @param speed The speed to move at (0-1023).
00377     ##
00378     ## @return 
00379     def setWheelSpeed(self, index, direction, speed):
00380         if speed > 1023:
00381             speed = 1023
00382         if direction == self.FORWARD:
00383             # 0~1023 is forward, it is stopped by setting to 0 while rotating to CCW direction.
00384             self.write(index, P_GOAL_SPEED_L, [speed%256, speed>>8])
00385         else:
00386             # 1024~2047 is backward, it is stopped by setting to 1024 while rotating to CW direction.
00387             speed += 1024
00388             self.write(index, P_GOAL_SPEED_L, [speed%256, speed>>8])
00389 
00390     ###########################################################################
00391     # Extended ArbotiX Driver
00392 
00393     ## Helper definition for analog and digital access.
00394     LOW = 0
00395     ## Helper definition for analog and digital access.
00396     HIGH = 0xff
00397     ## Helper definition for analog and digital access.
00398     INPUT = 0
00399     ## Helper definition for analog and digital access.
00400     OUTPUT = 0xff
00401 
00402     # ArbotiX-specific register table
00403     # We do Model, Version, ID, Baud, just like the AX-12
00404     ## Register base address for reading digital ports
00405     REG_DIGITAL_IN0 = 5
00406     REG_DIGITAL_IN1 = 6
00407     REG_DIGITAL_IN2 = 7
00408     REG_DIGITAL_IN3 = 8
00409     ## Register address for triggering rescan
00410     REG_RESCAN = 15
00411     # 16, 17 = RETURN, ALARM
00412     ## Register address of first analog port (read only).
00413     ## Each additional port is BASE + index.
00414     ANA_BASE = 18
00415     ## Register address of analog servos. Up to 10 servos, each
00416     ## uses 2 bytes (L, then H), pulse width (0, 1000-2000ms) (Write only)
00417     SERVO_BASE = 26
00418     # Address 46 is Moving, just like an AX-12
00419     REG_DIGITAL_OUT0 = 47
00420 
00421     ## @brief Force the ArbotiX2 to rescan the Dynamixel busses.
00422     def rescan(self):
00423         self.write(253, self.REG_RESCAN, [1,])
00424 
00425     ## @brief Get the value of an analog input pin.
00426     ##
00427     ## @param index The ID of the pin to read (0 to 7).
00428     ##
00429     ## @return 8-bit analog value of the pin, -1 if error.
00430     def getAnalog(self, index):
00431         try:
00432             return int(self.read(253, self.ANA_BASE+int(index), 1)[0])
00433         except:
00434             return -1
00435 
00436     ## @brief Get the value of an digital input pin.
00437     ##
00438     ## @param index The ID of the pin to read (0 to 31).
00439     ##
00440     ## @return 0 for low, 255 for high, -1 if error.
00441     def getDigital(self, index):
00442         try:
00443             if index < 32:
00444                 x = self.read(253, self.REG_DIGITAL_IN0 + int(index/8), 1)[0]
00445             else:
00446                 return -1
00447         except:
00448             return -1
00449         if x & (2**(index%8)):
00450             return 255
00451         else:
00452             return 0
00453 
00454     ## @brief Get the value of an digital input pin.
00455     ##
00456     ## @param index The ID of the pin to write (0 to 31).
00457     ##
00458     ## @param value The value of the port, >0 is high.
00459     ##
00460     ## @param direction The direction of the port, >0 is output.
00461     ##
00462     ## @return -1 if error.
00463     def setDigital(self, index, value, direction=0xff):
00464         if index > 31: return -1
00465         if value == 0 and direction > 0:
00466             self.write(253, self.REG_DIGITAL_OUT0 + int(index), [1])
00467         elif value > 0 and direction > 0:
00468             self.write(253, self.REG_DIGITAL_OUT0 + int(index), [3])
00469         elif value > 0 and direction == 0:
00470             self.write(253, self.REG_DIGITAL_OUT0 + int(index), [2])
00471         else:
00472             self.write(253, self.REG_DIGITAL_OUT0 + int(index), [0])
00473         return 0
00474 
00475     ## @brief Set the position of a hobby servo.
00476     ##
00477     ## @param index The ID of the servo to write (0 to 7).
00478     ##
00479     ## @param value The position of the servo in milliseconds (1500-2500). 
00480     ## A value of 0 disables servo output.
00481     ##
00482     ## @return -1 if error.
00483     def setServo(self, index, value):
00484         if index > 7: return -1
00485         if value != 0 and (value < 500 or value > 2500):
00486             print "ArbotiX Error: Servo value out of range:",val
00487         else:
00488             self.write(253, self._SERVO_BASE + 2*index, [value%256, value>>8])
00489         return 0
00490 


arbotix_python
Author(s): Michael Ferguson
autogenerated on Sun Oct 5 2014 22:16:05