element_driver.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 """
00004     A Python library for the cmRobot Element microcontroller.
00005     Created for The Pi Robot Project: http://www.pirobot.org
00006     Copyright (c) 2012 Patrick Goebel.  All rights reserved.
00007 
00008     This program is free software; you can redistribute it and/or modify
00009     it under the terms of the GNU General Public License as published by
00010     the Free Software Foundation; either version 2 of the License, or
00011     (at your option) any later version.
00012     
00013     This program is distributed in the hope that it will be useful,
00014     but WITHOUT ANY WARRANTY; without even the implied warranty of
00015     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00016     GNU General Public License for more details at:
00017     
00018     http://www.gnu.org/licenses/gpl.html
00019       
00020     NOTE: See the offical Element documentation at:
00021     
00022     http://www.cmrobot.com/#!element
00023     
00024     See the example files for more details.
00025 """
00026 
00027 import thread
00028 import math
00029 import os
00030 import time
00031 import sys
00032 from serial.serialutil import SerialException
00033 from serial import Serial
00034 
00035 class Element:  
00036     ''' Configuration Parameters
00037     '''    
00038     N_ANALOG_PORTS = 6
00039     N_DIGITAL_PORTS = 12
00040     
00041     MILLISECONDS_PER_PID_LOOP = 1.6 # Do not change this!  It is a fixed property of the Element PID controller.
00042     
00043     pi_robot = False                # Special flag set True when using Pi Robot
00044     if pi_robot:
00045         default_pid_params['units'] = 0                   # 1 is inches, 0 is metric (cm for sensors, meters for wheels measurements) and 2 is "raw"
00046         default_pid_params['wheel_diameter'] = 0.132      # meters (5.0 inches) meters or inches depending on UNITS
00047         default_pid_params['wheel_track'] = 0.3365        # meters (12.8 inches) meters or inches units depending on UNITS
00048         default_pid_params['encoder_resolution'] = 624    # encoder ticks per revolution of the wheel without external gears
00049         default_pid_params['gear_reduction'] = 1.667      # This is for external gearing if you have any.  In this case there is a 60/36 tooth gear ratio.
00050         
00051         default_pid_params['encoder_type'] = 1            # 1 = quadrature, 0 = single
00052         default_pid_params['motors_reversed'] = True      # Multiplies encoder counts by -1 if the motor rotation direction is reversed.
00053     
00054         default_pid_params['init_pid'] = False # Set to True if you want to update UNITS, VPID and DPID parameters.  Otherwise, those stored in the Element's firmware are used.**
00055     
00056         default_pid_params['VPID_P'] = 2   # Proportional
00057         default_pid_params['VPID_I'] = 0   # Integral
00058         default_pid_params['VPID_D'] = 5   # Derivative                                                                               
00059         default_pid_params['VPID_L'] = 45  # Loop: this together with UNITS and WHEEL_DIAMETER determines real-world velocity
00060         
00061         default_pid_params['DPID_P'] = 1   # Proportional
00062         default_pid_params['DPID_I'] = 0   # Integral
00063         default_pid_params['DPID_D'] = 0   # Derivative 
00064         default_pid_params['DPID_A'] = 5   # Acceleration
00065         default_pid_params['DPID_B'] = 10  # Dead band
00066     else:
00067         default_pid_params = dict()
00068     
00069     # A constant to test for corrupt sensor readings (if any)
00070     BAD_VALUE = -999
00071     
00072     def __init__(self, port="/dev/ttyUSB0", baudrate=19200, timeout=0.5, units=0):
00073         self.port = port
00074         self.baudrate = baudrate
00075         self.timeout = timeout
00076         self.units = units
00077         self.encoder_count = 0
00078         self.writeTimeout = timeout
00079         self.interCharTimeout = timeout / 30.
00080         self.count = 0
00081         self.shutdown = False
00082     
00083         # Keep things thread safe
00084         self.mutex = thread.allocate_lock()
00085             
00086         # An array to cache analog sensor readings
00087         self.analog_sensor_cache = [None] * self.N_ANALOG_PORTS
00088         
00089         # An array to cache digital sensor readings
00090         self.digital_sensor_cache = [None] * self.N_DIGITAL_PORTS
00091     
00092     def connect(self, use_base_controller=False, pid_params=default_pid_params):
00093         try:
00094             print "Connecting to Element on port", self.port, "..."
00095             self.port = Serial(port=self.port, baudrate=self.baudrate, timeout=self.timeout, writeTimeout=self.writeTimeout)
00096             # The next line is necessary to give the firmware time to wake up.
00097             time.sleep(1)
00098             test = self.get_baud()
00099             if test != self.baudrate:
00100                 time.sleep(1)
00101                 test = self.get_baud()
00102                 if test != self.baudrate:
00103                     raise SerialException
00104             print "Connected at", self.baudrate, "baud.  Main voltage:", self.voltage()
00105             print "Element is ready."
00106 
00107         except SerialException:
00108             print "Cannot connect to Element!"
00109             print "Make sure you are plugged in and turned on."
00110             os._exit(1)
00111             
00112         # A bug in the Element's firmware prevents the units value from being saved in EEPROM
00113         # so we always have to set it here.
00114         if self.get_units() != self.units:
00115             self.set_units(self.units)
00116             
00117     def setup_base_controller(self, pid_params):
00118         # Check to see if any PID parameters are missing
00119         for param in pid_params:
00120             if pid_params[param] == "":
00121                 print("*** PID Parameter " + param + " is missing. ***")
00122                 os._exit(1)
00123                 
00124         self.wheel_diameter = pid_params['wheel_diameter']
00125         self.wheel_track = pid_params['wheel_track']
00126         self.encoder_resolution = pid_params['encoder_resolution']
00127         self.encoder_type = pid_params['encoder_type']
00128         self.gear_reduction = pid_params['gear_reduction']
00129         self.motors_reversed = pid_params['motors_reversed']
00130 
00131         self.init_pid = pid_params['init_pid']
00132         self.VPID_P = pid_params['VPID_P']
00133         self.VPID_I = pid_params['VPID_I']
00134         self.VPID_D  = pid_params['VPID_D']
00135         self.VPID_L = pid_params['VPID_L']
00136         self.DPID_P = pid_params['DPID_P']
00137         self.DPID_I = pid_params['DPID_I']
00138         self.DPID_D = pid_params['DPID_D']
00139         self.DPID_A = pid_params['DPID_A']
00140         self.DPID_B = pid_params['DPID_B']
00141 
00142         self.loop_interval = None
00143         self.ticks_per_meter = None
00144         
00145         if pid_params['init_pid']:
00146             # If the init_pid parameter is set, store the new parameters in EEPROM.
00147             self.init_PID()
00148         else:
00149             # Otherwise, read the parameters from EEPROM.
00150             self.units = self.get_units()
00151             [self.VPID_P, self.VPID_I, self.VPID_D, self.VPID_L] = self.get_vpid()
00152             [self.DPID_P, self.DPID_I, self.DPID_D, self.DPID_A, self.DPID_B] = self.get_dpid()
00153         
00154         if self.units == 0:
00155             self.ticks_per_meter = int(self.encoder_resolution * self.gear_reduction  / (self.wheel_diameter * math.pi))
00156         elif self.units == 1:
00157             self.ticks_per_meter = int(self.encoder_resolution * self.gear_reduction / (self.wheel_diameter * math.pi * 2.54 / 100.0))
00158             
00159         self.loop_interval = self.VPID_L * self.MILLISECONDS_PER_PID_LOOP / 1000
00160         
00161             
00162     def init_PID(self):
00163         print "Storing Units and PID parameters in EEPROM"
00164         self.set_encoder(self.encoder_type)
00165         self.set_units(self.units)
00166         self.set_vpid(self.VPID_P, self.VPID_I, self.VPID_D, self.VPID_L)
00167         self.set_dpid(self.DPID_P, self.DPID_I, self.DPID_D, self.DPID_A, self.DPID_B)
00168 
00169     def open(self): 
00170         ''' Open the serial port.
00171         '''
00172         self.port.open()
00173 
00174     def close(self): 
00175         ''' Close the serial port.
00176         '''
00177         self.port.close() 
00178     
00179     def send(self, cmd):
00180         ''' This command should not be used on its own: it is called by the execute commands
00181             below in a thread safe manner.
00182         '''
00183         self.port.write(cmd + '\r')
00184 
00185     def recv(self, timeout=0.5):
00186         timeout = min(timeout, self.timeout)
00187         ''' This command should not be used on its own: it is called by the execute commands   
00188             below in a thread safe manner.  Note: we use read() instead of readline() since
00189             readline() tends to return garbage characters from the Element
00190         '''
00191         c = ''
00192         value = ''
00193         attempts = 0
00194         while c != '>':
00195             c = self.port.read(1)
00196             value += c
00197             attempts += 1
00198             if attempts * self.interCharTimeout > timeout:
00199                 return None
00200 
00201         value = value.strip('>')
00202 
00203         return value
00204             
00205     def recv_ack(self):
00206         ''' This command should not be used on its own: it is called by the execute commands
00207             below in a thread safe manner.
00208         '''
00209         ack = self.recv(self.timeout)
00210         return ack == 'ACK'
00211 
00212     def recv_int(self):
00213         ''' This command should not be used on its own: it is called by the execute commands
00214             below in a thread safe manner.
00215         '''
00216         value = self.recv(self.timeout)
00217         try:
00218             return int(value)
00219         except:
00220             return None
00221 
00222     def recv_array(self):
00223         ''' This command should not be used on its own: it is called by the execute commands
00224             below in a thread safe manner.
00225         '''
00226         try:
00227             values = self.recv(self.timeout * self.N_ANALOG_PORTS).split()
00228             return map(int, values)
00229         except:
00230             return []
00231 
00232     def execute(self, cmd):
00233         ''' Thread safe execution of "cmd" on the Element returning a single value.
00234         '''
00235         self.mutex.acquire()
00236         
00237         try:
00238             self.port.flushInput()
00239         except:
00240             pass
00241         
00242         ntries = 1
00243         attempts = 0
00244         
00245         try:
00246             self.port.write(cmd + '\r')
00247             value = self.recv(self.timeout)
00248             while attempts < ntries and (value == '' or value == 'NACK' or value == None):
00249                 try:
00250                     self.port.flushInput()
00251                     self.port.write(cmd + '\r')
00252                     value = self.recv(self.timeout)
00253                 except:
00254                     print "Exception executing command: " + cmd
00255                 attempts += 1
00256         except:
00257             self.mutex.release()
00258             if not self.shutdown:
00259                 print "Exception executing command: " + cmd
00260             value = None
00261         
00262         self.mutex.release()
00263         return value
00264 
00265     def execute_array(self, cmd):
00266         ''' Thread safe execution of "cmd" on the Element returning an array.
00267         '''
00268         self.mutex.acquire()
00269         
00270         try:
00271             self.port.flushInput()
00272         except:
00273             pass
00274         
00275         ntries = 1
00276         attempts = 0
00277         
00278         try:
00279             self.port.write(cmd + '\r')
00280             values = self.recv_array()
00281             while attempts < ntries and (values == '' or values == ['NACK'] or values == [] or values == None):
00282                 try:
00283                     self.port.flushInput()
00284                     self.port.write(cmd + '\r')
00285                     values = self.recv_array()
00286                 except:
00287                     print("Exception executing command: " + cmd)
00288                 attempts += 1
00289         except:
00290             self.mutex.release()
00291             print "Exception executing command: " + cmd
00292             if not self.shutdown:
00293                 raise SerialException
00294             return []
00295         
00296         try:
00297             values = map(int, values)
00298         except:
00299             values = []
00300 
00301         self.mutex.release()
00302         return values
00303         
00304     def execute_ack(self, cmd):
00305         ''' Thread safe execution of "cmd" on the Element returning True if response is ACK.
00306         '''
00307         self.mutex.acquire()
00308         
00309         try:
00310             self.port.flushInput()
00311         except:
00312             pass
00313         
00314         ntries = 1
00315         attempts = 0
00316         
00317         try:
00318             self.port.write(cmd + '\r')
00319             ack = self.recv(self.timeout)
00320             while attempts < ntries and (ack == '' or ack == 'NACK' or ack == None):
00321                 try:
00322                     self.port.flushInput()
00323                     self.port.write(cmd + '\r')
00324                     ack = self.recv(self.timeout)
00325                 except:
00326                     print "Exception executing command: " + cmd
00327                 attempts += 1
00328         except:
00329             self.mutex.release()
00330             if not self.shutdown:
00331                 print "execute_ack exception when executing", cmd
00332                 print sys.exc_info()
00333             return 0
00334         
00335         self.mutex.release()
00336         return ack == 'ACK'
00337         
00338     def execute_int(self, cmd):
00339         ''' Thread safe execution of "cmd" on the Element returning an int.
00340         '''
00341         self.mutex.acquire()
00342         
00343         try:
00344             self.port.flushInput()
00345         except:
00346             print "Flush exception!"
00347         
00348         ntries = 1
00349         attempts = 0
00350         
00351         try:
00352             self.port.write(cmd + '\r')
00353             value = self.recv(self.timeout)
00354             while attempts < ntries and (value == '' or value == 'NACK' or value == None):
00355                 try:
00356                     self.port.flushInput()
00357                     self.port.write(cmd + '\r')
00358                     value = self.recv(self.timeout)
00359                 except:  
00360                     print "Write/Flush exception!"
00361                 attempts += 1
00362         except:
00363             self.mutex.release()
00364             if not self.shutdown:
00365                 print "execute_int exception when executing", cmd
00366                 print sys.exc_info()
00367             return None
00368         
00369         try:
00370             value = int(value)
00371         except:
00372             value = None
00373             
00374         self.mutex.release()
00375         return value                   
00376                 
00377     def update_digital_cache(self, id, value):
00378         if value != "NACK":
00379             self.digital_sensor_cache[id] = value
00380             
00381     def update_analog_cache(self, id, value):
00382         if value != "NACK":
00383             self.analog_sensor_cache[id] = value
00384                 
00385     def get_analog_cache(self, ids):
00386         values = list()
00387         for id in ids:
00388             values.append(self.analog_sensor_cache[id])
00389         return values
00390 
00391     def fw(self):
00392         ''' The fw command returns the current firmware version.
00393         '''
00394         return self.execute('fw')
00395 
00396     def reset(self):
00397         ''' The reset command resets the Element board and reboots it. You
00398             will see the Element welcome screen appear after a short delay.
00399             Once the welcome string appears, the Element is ready to accept
00400             commands.
00401         '''
00402         return self.execute('reset')
00403 
00404     def blink_led(self, id, rate):
00405         ''' Usage 1: blink_led(id, rate)  
00406             Usage 2: blink_led([id1, id2], [rate1, rate2]) 
00407             The blink_led command can blink one of the two onboard green LEDs
00408             simultaneously, or individually. Each complex parameter is comprised
00409             of an <ledId:blinkRate> pair. The ledId specifies which of the two
00410             green LEDs to blink, and blinkRate specifies the delay between blinks.
00411             The minimum blink rate is 1, and the largest is 127. A value of 0 turns
00412             the led off.
00413         '''
00414         if type(id) == int: id = [id]
00415         if type(rate) == int: rate = [rate]          
00416         return self.execute_ack('blink %s' %' '.join(map(lambda x: '%d:%d' %x, zip(id,rate))))
00417 
00418     def get_compass(self, i2c_addr=None):
00419         ''' Usage 1: heading = get_compass()
00420             Usage 2: heading = get_compass(i2c_addr)           
00421             The get_compass command queries a Devantech CMPS03 Electronic
00422             compass module attached to the Elements I2C port.
00423             The current heading is returned in Binary Radians, or BRADS.To
00424             convert BRADS to DEGREES, multiply BRADS by 360/255 (~1.41).
00425             The default I2C address is 0xC0, however another I2C address can be
00426             supplied as an optional parameter.
00427         '''
00428         return 360. / 255. * self.execute_int('cmps03 %X' %i2c_addr if i2c_addr else 'cmps03' )
00429 
00430     def get_encoder(self):
00431         ''' The get_encoder command returns the encoder type for the Element,
00432             single (0) or quadrature (1).
00433         '''
00434         return self.execute_int('cfg enc');
00435 
00436     def set_encoder(self, encoder_type):
00437         ''' The set_encoder command configures the internal encoder type to be either
00438             single (0) or quadrature (1) type. This information is saved in the
00439             EEPROM, so that the configuration will be retained after a reboot. If
00440             you are using a quadrature encoder (dual channels), and the Element
00441             is configured for single encoder operation, then the second quadrature
00442             channel will be ignored. Thus make sure the correct encoder type is
00443             configured according to your setup. The cfg enc command without a
00444             parameter returns the value currently stored in EEPROM.
00445         '''
00446         return self.execute_ack('cfg enc %d' %encoder_type);
00447 
00448     def get_baud(self):
00449         ''' Get the current baud rate on the serial port.
00450         '''
00451         return self.execute_int('cfg baud');
00452 
00453     def set_baud(self, baudrate):
00454         ''' The set_baud command configures the serial baud rate on the
00455             Element. Values can be 0=2400, 1=4800, 2=9600, 3=19200,
00456             4=57600, or 5=115200. You can also type in the actual baud rate
00457             string as well (e.g. 19200). The default baud rate used to
00458             communicate with the Element is 19200. The cfg baud command
00459             without a parameter returns the value currently stored in EEPROM.
00460         '''
00461         return self.execute_ack('cfg baud %d' %baudrate);
00462 
00463     def get_units(self):
00464         ''' The get_units returns the current units used for sensor
00465             readings. Values are 0 for metric mode, 1 for English mode, and 2 for
00466             raw mode.  In raw mode, srf04, srf05, pping, and maxez1 return
00467             reading in units of 0.4us. srf08 and srf10 return readings of 1us..
00468         '''
00469         return self.execute_int('cfg units')
00470 
00471     def set_units(self, units):
00472         ''' The set_units command sets the internal units used for sensor
00473             readings. Values are 0 for metric mode, 1 for English mode, and 2 for
00474             raw mode.  In raw mode, srf04, srf05, pping, and maxez1 return
00475             reading in units of 0.4us. srf08 and srf10 return readings of 1us.
00476             The cfg units command without a parameter returns the value
00477         '''
00478         self.units = units
00479         return self.execute_ack('cfg units %d' %units);
00480 
00481     def get_encoder_count(self, id):
00482         ''' Usage 1: get_encoder_count(id)
00483             Usage 2: get_encoder_count([id1, id2])
00484             The get_encoder_count command returns the values of the encoder
00485             count (channel B) for the specified encoder Id(s). NOTE: The encoder
00486             counts for channel A are used for internal VPID and DPID algorithms.
00487         '''
00488         if type(id) == int: id=[id]
00489         values = self.execute_array('getenc %s' %' '.join(map(str, id)))
00490         if len(values) != len(id):
00491             print "Encoder count did not match ID count for ids", id
00492             raise SerialException
00493         else:
00494             if self.motors_reversed:
00495                 for i in range(len(id)):
00496                     values[i] = -1 * values[i]          
00497         return values
00498 
00499     def clear_encoder(self, id):
00500         ''' Usage 1: clear_encoder(id)
00501             Usage 2: clear_encoder([id1, id2])
00502             The clear_encoder command clears the values of the encoder
00503             count (channel B) for the specified encoder Id.
00504         '''
00505         if type(id) == int: id=[id]
00506         return self.execute_ack('clrenc %s' %' '.join(map(str, id)))
00507 
00508     def get_io(self, id):
00509         ''' Usage 1: get_io(id)
00510             Usage 2: get_io([id1, id2, id3, ... , idN]) 
00511             The get_io command changes the pin, pinId (range 0-12), to an input
00512             (if it was an output), and gets the value of the specified General
00513             Purpose I/O lines on the Element. The valid range of I/O pin Ids is
00514             0 thru 12. More than one pid can be specified by enter a list as argument.
00515         '''
00516         if type(id) == int: id=[id]
00517         values = self.execute_array('getio %s' %' '.join(map(str, id)))
00518         n = len(id)
00519         for i in range(n):
00520             self.update_digital_cache(id[i], values[i])
00521         if n == 1:
00522             return values[0]
00523         else:
00524             return values
00525 
00526     def set_io(self, id, val):
00527         ''' Usage 1: set_io(id, val)
00528             Usage 2: set_io([id1, id2, ... , idN], [val1, val2, ..., valN)  
00529             The set_io command sets the specified General Purpose I/O line pinId
00530             (range 0-12) on the Element to the specified value. Each complex
00531             parameter is a <pinId:value> pair, where the valid range of pinId is 0
00532             thru 12, and value can be 0 or 1 which corresponds to 0v or +5V
00533             respectively.  Also I/O lines 4,5,6,7, 8 and 9 cannot be used if you
00534             have servos connected to them. Pin 10, 11, and 12 correspond to the
00535             internal h-bridge enable, SCL, and SDA respectively.
00536         '''
00537         if type(id) == int: id = [id]
00538         if type(val) == int: val = [val]          
00539         return self.execute_ack('setio %s' %' '.join(map(lambda x: '%d:%d' %x, zip(id, val))))
00540 
00541     def get_maxez1(self, triggerPin, outputPin):
00542         ''' The maxez1 command queries a Maxbotix MaxSonar-EZ1 sonar
00543             sensor connected to the General Purpose I/O lines, triggerPin, and
00544             outputPin, for a distance, and returns it in Centimeters. NOTE: MAKE
00545             SURE there's nothing directly in front of the MaxSonar-EZ1 upon
00546             power up, otherwise it wont range correctly for object less than 6
00547             inches away! The sensor reading defaults to use English units
00548             (inches). The sensor distance resolution is integer based. Also, the
00549             maxsonar trigger pin is RX, and the echo pin is PW.
00550         '''
00551         return self.execute_int('maxez1 %d %d' %(triggerPin, outputPin))
00552 
00553     def stop(self):
00554         ''' Stop both motors.
00555         '''
00556         return self.execute_ack('stop')
00557 
00558     def get_vpid(self):
00559         ''' Get the PIDL parameter values.
00560         '''       
00561         pidl = self.execute('vpid')
00562         return map(lambda x: int(x.split(':')[1]), pidl.split())
00563 
00564     def set_vpid(self, prop, integ, deriv, loop):
00565         ''' The set_vpid command sets the PIDL (Proportional, Integral,
00566             Derivative, and Loop) parameters for the Velocity PID control on the
00567             Element. The PIDL parameters are parsed, and saved (in
00568             eeprom). For more information on PIDL control, see the PIDL
00569             configuration section below.  By default the Element VPID
00570             parameters are configured to work with our Traxster Robot Kit
00571         '''
00572         [self.VPID_P, self.VPID_I, self.VPID_D, self.VPID_L] = [prop, integ, deriv, loop]
00573         return self.execute_ack('vpid %d:%d:%d:%d' %(prop, integ, deriv, loop))
00574     
00575     def digo(self, id, dist, vel):
00576         ''' Usage 1: m(id, dist, vel)
00577             Usage 2: digo([id1, id2], [dist1, dist2], [vel1, vel2]) 
00578             Simply put, the digo command allows you to command your robot to
00579             travel a specified distance, at a specified speed. This command uses
00580             the internal VPID and DPID algorithms to control velocity and distance.
00581             Therefore, you must have dual motors, and dual wheel encoders
00582             connected to the Element motor ports and encoder inputs.
00583         '''
00584         if type(id) == int: id = [id]
00585         if type(dist) == int: id = [dist]
00586         if type(vel) == int: vel = [vel]
00587         
00588         if self.motors_reversed:
00589             for i in range(len(dist)):
00590                 dist[i] = -1 * dist[i] 
00591                 
00592         return self.execute('digo %s' %' '.join(map(lambda x: '%d:%d:%d' %x, zip(id, dist, vel))))
00593     
00594     def digo_m_per_s(self, id, dist, vel):
00595         ''' Identical to digo but specifies velocities in meters per second.
00596         '''
00597         if type(id) == int: id = [id]
00598         if type(dist) == int: id = [dist]
00599         if type(vel) == int: vel = [vel]
00600         
00601         spd = list()
00602         for v in vel:
00603             if self.units == 0:
00604                 revs_per_second = float(v) / (self.wheel_diameter * math.pi)
00605             elif self.units == 1:
00606                 revs_per_second = float(v) / (self.wheel_diameter * math.pi * 2.54 / 100)
00607             ticks_per_loop = revs_per_second * self.encoder_resolution * self.loop_interval * self.gear_reduction
00608             spd.append(int(ticks_per_loop))
00609                  
00610         return self.execute('digo %s' %' '.join(map(lambda x: '%d:%d:%d' %x, zip(id, dist, spd))))
00611     
00612     def get_dpid(self):
00613         ''' Get the PIDA parameter values.
00614         '''  
00615         dpid = self.execute('dpid')
00616         return map(lambda x: int(x.split(':')[1]), dpid.split())
00617 
00618     def set_dpid(self, prop, integ, deriv, accel, dead_band):
00619         ''' The set_dpid command gets/sets the PIDA (Proportional, Integral,
00620             Derivative, and Acceleration) parameters for the distance PID control
00621             on the Element. If the PIDA parameters are absent, the PIDA
00622             values are returned. Otherwise the PIDA parameters are parsed, and
00623             saved (in eeprom).  
00624         '''
00625         [self.DPID_P, self.DPID_I, self.DPID_D, self.DPID_A, self.DPID_B] = [prop, integ, deriv, accel, dead_band]
00626         return self.execute_ack('dpid %d:%d:%d:%d:%d' %(prop, integ, deriv, accel, dead_band))
00627      
00628     def set_rpid(self, r):
00629         ''' The rpid command sets the default PID params known to work with
00630             either the Stinger or Traxster Robotic Kits in the firmware. This makes
00631             it quick and easy to set up the PID params for both robots.
00632         '''
00633         return self.execute_ack('rpid %c' %r)
00634 
00635     def get_pids(self):
00636         ''' Once a digo command is issued, an internal state variable within the
00637             firmware is set to 1, and it stays in that state until the algorithm has
00638             completed. Upon completion, the state is set to 0. The pids
00639             command simply returns the value of the internal variable to
00640             determine if the algorithms is currently busy, or if it has finished, thus
00641             allowing subsequent digo commands to be issued w/o clobbering
00642             previous ones.
00643         '''
00644         return self.execute_int('pids');
00645     
00646     def pwm(self, id, vel, rate=None):
00647         ''' Usage 1: pwm(id, vel)
00648             Usage 2: pwm(id, vel, rate=r)
00649             Usage 3: pwm([id1, id2], [vel1, vel2]) 
00650             Usage 4: pwm([id1, id2], [vel1, vel2], rate=r)   
00651             The pwm command sets the Pulse Width Modulation value for
00652             Motor 1 & Motor 2. Each complex parameter is a motor
00653             <motorId:pwm value> pair, where the motor id can be 1 or 2, and the
00654             pwm value can be -100 to 100. Each complex parameter pair is
00655             separated by one or more spaces.
00656             The optional rate parameter allows the
00657             motor(s) speed(s)s to be ramped up or down to the specified speed
00658             from the current motor speed.
00659         '''
00660         if type(id) == int: id = [id]
00661         if type(vel) == int: vel = [vel]          
00662         if rate != None:
00663             cmd = 'pwm r:%d ' %rate + ' '.join(map(lambda x: '%d:%d' %x, zip(id, vel)))
00664         else:         
00665             cmd = 'pwm %s' % ' '.join(map(lambda x: '%d:%d' %x, zip(id, vel)))
00666         return self.execute(cmd)
00667 
00668     def step(self, dir, speed ,steps):
00669         ''' The step command is used to step a bipolar stepper motor in direction
00670             dir, at the specified speed, for the specified number of steps.
00671             The dir parameter specifies a CW or CCW rotational direction, and its
00672             value can be either 0 (CCW) or 1(CW). Your specific direction is based
00673             on the way that you have your bipolar motor connected to the
00674             Element.  The speed parameter can be a value from 0 to 100.
00675             The steps parameter specifies the maximum number of steps to take.
00676             A value of 0 means step infinitely. Internally, this number is stored in
00677             an unsigned 32 bit variable, so the user can specify a larger number of
00678             steps.
00679         '''
00680         return self.execute_ack('step %d %d %d' %(dir, speed, steps))
00681 
00682     def sweep(self, id, speed, steps):
00683         ''' The sweep command is used to sweep a bipolar motor, for step
00684             number of steps, at speed (0-100), thus providing a sweeping motion.
00685             The initial rotational direction of sweep is in the CW direction.
00686             Upon initial receipt of the command, the firmware will sweep the
00687             motor for 1/2 of the number of steps specified, starting in a CW
00688             direction. Once that number of steps has occurred, the sweep
00689             direction will change, and subsequent sweeps will rotate for the full
00690             amount of steps.     Thus, the starting point for the motor is in the
00691             middle of each sweep. You may stop the sweep by either issuing a sweep 
00692             command w a 0 speed, or simply sending a stop command.
00693         '''
00694         return self.execute_ack('sweep %d %d %d' %(id, speed, steps))
00695             
00696     def sensor(self, id):
00697         ''' Usage 1: reading = sensor(id)
00698             Usage 2: readings = sensor([id1, id2, ..., idN])
00699             The sensor command returns the raw A/D (8 bit) reading from the
00700             analog sensor ports 0-5. Multiple values can be read at a time by
00701             specifying multiple pins as a list. Pin 5 is 1/3 of
00702             the voltage of the power supply for the Element. To calculate the
00703             battery voltage, simply multiply the value returned by Sensor 5 by 
00704             15/1024.
00705         '''
00706         
00707         if type(id) == int: id = [id]
00708         cmd = 'sensor %s' %' '.join(map(str, id))
00709         values = self.execute_array(cmd)
00710         n = len(values)
00711         if n != len(id):
00712             print "Array size incorrect: returning cached values for sensors", id
00713             values = self.get_analog_cache(id)
00714             if len(values) == 1:
00715                 return values[0]
00716             else:
00717                 return values
00718         try:
00719             for i in range(n):
00720                 if values[i] == None:
00721                     values[i] = self.BAD_VALUE
00722             for i in range(n):
00723                 self.update_analog_cache(id[i], values[i])
00724             if n == 1:
00725                 return values[0]
00726             else:
00727                 return values
00728         except:
00729             print "Exception reading analog sensors: returning cached values for sensors", id
00730             return self.analog_sensor_cache
00731         
00732         
00733     def get_analog(self, id):
00734         return self.sensor(id)
00735     
00736     def get_all_analog(self):
00737         ''' Return the readings from all analog ports.
00738         '''
00739         values = self.sensor(range(self.N_ANALOG_PORTS))
00740         return values
00741 
00742     def servo(self, id, pos):
00743         ''' Usage 1: servo(id, pos)
00744             Usage 2: servo([id1, id2, ... , idN], [pos1, pos2, ..., posN)     
00745             The servo command sets a servo connected to General Purpose I/O
00746             port the specified position. The value of the position can range from 
00747             -99 to 100, where 0 is the center position. Setting the position to -100
00748             will disable the servo, allowing it to turn freely by hand.
00749             Each parameter is a <servo id:position> pair, where the servo
00750             id can be 1,2,3,4,5, or 6.
00751         '''
00752         if type(id) == int: id = [id]
00753         if type(pos) == int: pos = [pos]          
00754         return self.execute_ack('servo %s' %' '.join(map(lambda x: '%d:%d' %x, zip(id, pos))))
00755 
00756     def sp03(self, msg, i2c_addr):
00757         ''' Usage1: sp03(msg)
00758             Usage2: sp03(msg, ic2_addr)
00759             The sp03 command instructs a Devantech SP03 Speech Synthesizer to
00760             speak the appropriate phrase. If a character representing a number in
00761             the range of 0 to 30, then the SP03 will speak previously programmed
00762             canned phrases. If a phrase is sent, then it will speak the phrase. An
00763             optional I2C address can also be specified. Otherwise, the default I2C
00764             address of 0xC4.
00765          '''
00766         return not self.execute_ack('sp03 0x%X %s' %(i2c_addr, msg) if i2c_addr else 'sp03 %s' %msg)
00767 
00768     def srf04(self, triggerPin, outputPin):
00769         ''' The srf04 command queries an SRF04 sonar sensor
00770             connected to the General Purpose I/O lines triggerPin and outputPin,
00771             for a distance and returns it in the units configured (default is English
00772             inches). If the Element units are configured (using cfg units) for raw mode,
00773             srf04 returns readings in units of 0.4us, and the max distance returned
00774             is 65000 (out of range). When configured for English units, max
00775             distance returned is 100 inches (out of range), and when configured
00776             for Metric units, max distance returned is 255 (out of range). NOTE:
00777             Sonar distance resolution is integer based.
00778         '''
00779         return self.execute_int('srf04 %d %d' %(triggerPin, outputPin))
00780 
00781     def srf05(self, pinId):
00782         ''' The srf05/Ping command queries an SRF05/Ping sonar sensor
00783             connected to the General Purpose I/O line pinId for a distance,
00784             and returns it in the units configured (default is English - inches).
00785             If the Element units are configured (using cfg units) for raw mode,
00786             pping and srf05 return readings in units of 0.4us, and the max
00787             distance returned is 65000 (out of range). When configured for
00788             English units, max distance returned is 100 inches (out of range), and
00789             when configured for Metric units, max distance returned is 255 (out of
00790             range). Sonar distance resolution is integer based.
00791         '''
00792         return self.execute_int('srf05 %d' %pinId);
00793 
00794     def pping(self, pinId):
00795         ''' The srf05/Ping command queries an SRF05/Ping sonar sensor
00796             connected to the General Purpose I/O line pinId for a distance,
00797             and returns it in the units configured (default is English - inches).
00798             If the Element units are configured (using cfg units) for raw mode,
00799             pping and srf05 return readings in units of 0.4us, and the max
00800             distance returned is 65000 (out of range). When configured for
00801             English units, max distance returned is 100 inches (out of range), and
00802             when configured for Metric units, max distance returned is 255 (out of
00803             range). Sonar distance resolution is integer based.
00804         '''
00805         value = self.execute_int('pping %d' %pinId);
00806         self.update_digital_cache(pinId, value)
00807         return value
00808 
00809     def srf08(self, i2c_addr=None):
00810         ''' Usage1: srf08()
00811             Usage2: srf08(ic2_addr)
00812             The srf08/srf10 command queries a Devantech SRF08/SRF10 sonar
00813             sensor at address i2c_addr for a distance reading in the units
00814             configured (default is English - inches).  The i2cAddr parameter is
00815             optional, and defaults to 0xE0 for both sensors. The i2c address can
00816             be changed for any i2c module using the i2cp command.  Sonar
00817             distance resolution is integer based.  If the Element units are 
00818             configured (using cfg units) for raw mode, srf08 and srf10 return
00819             readings in units of 1us.
00820          '''
00821         return self.execute_int('srf08 0x%X' %i2c_addr if i2c_addr else 'srf08')
00822 
00823     def srf10(self, i2caddr=None):
00824         ''' Usage1: srf10()
00825             Usage2: srf10(ic2_addr)
00826             The srf08/srf10 command queries a Devantech SRF08/SRF10 sonar
00827             sensor at address ic2_addr for a distance reading in the units
00828             configured (default is English - inches). The ic2_addr parameter is
00829             optional, and defaults to 0xE0 for both sensors. The i2c address can
00830             be changed for any i2c module using the i2cp command. Sonar
00831             distance resolution is integer based. If the Element units are
00832             configured (using cfg units) for raw mode, srf08 and srf10 return
00833             readings in units of 1us.
00834          '''
00835         return self.execute_int('srf10 0x%X' %i2caddr if i2caddr else 'srf10')
00836 
00837     def tpa81(self, i2caddr=None):
00838         ''' Usage1: tpa81()
00839             Usage2: tpa81(ic2_addr) 
00840             The tpa81 command queries a Devantech TPA81 thermopile sensor for
00841             temperature values. It returns 8 temperature values.
00842         '''
00843         return self.execute_array('tpa81 0x%X' %i2caddr if i2caddr else 'tpa81')
00844 
00845     def vel(self):
00846         ''' The vel command returns the left and right wheel velocities. The
00847             velocity returned is based on the PIDL parameter configuration.  The
00848             units are encoder ticks per PID loop interval.
00849         '''
00850         return self.execute_array('vel')
00851     
00852     def vel_m_per_s(self):
00853         ''' Return the left and right wheel velocities in meters per second.
00854         '''
00855         [left_ticks_per_loop, right_ticks_per_loop] = self.vel()
00856         left_revs_per_second = float(left_ticks_per_loop) / self.encoder_resolution / self.loop_interval
00857         right_revs_per_second = float(right_ticks_per_loop) / self.encoder_resolution / self.loop_interval
00858         if self.units == 0:
00859             left_m_s = left_revs_per_second * self.wheel_diameter * math.pi
00860             right_m_s = right_revs_per_second * self.wheel_diameter * math.pi
00861         elif self.units == 1:
00862             left_m_s = left_revs_per_second * self.wheel_diameter * 2.54 * math.pi / 100
00863             right_m_s = right_revs_per_second * self.wheel_diameter * 2.54 * math.pi / 100
00864         return list([left_m_s, right_m_s])
00865     
00866     def vel_mph(self):
00867         ''' Return the left and right wheel velocities in miles per hour.
00868         '''
00869         [left_m_s, right_m_s] = self.vel_m_per_s()
00870         m_s_2_mph = 2.25
00871         return list([left_m_s * m_s_2_mph, right_m_s * m_s_2_mph])
00872     
00873     def vel_fps(self):
00874         ''' Return the left and right wheel velocities in feet per second.
00875         '''
00876         [left_m_s, right_m_s] = self.vel_m_per_s()
00877         m_s_2_fps = 3.2808
00878         return list([left_m_s * m_s_2_fps, right_m_s * m_s_2_fps]) 
00879 
00880     def restore(self):
00881         ''' Restores the factory default settings, and resets the board. NOTE:
00882             This will erase any configurations you have saved to EEPROM,
00883             including VPID, DPID, and baud rate settings.
00884         '''
00885         print self.execute('restore')
00886 
00887     def line(self, addr, newaddr=None, seven=False):
00888         ''' Queries a RoboticsConnection Line Following Sensor at address addr.
00889             If the -a option is specified, then the address of the module will be
00890             changed to the new address associated w the -a switch.
00891             If the optional 7 is appended to the end of the line command, e.g.
00892             line7, then two additional values will be returned from those Line
00893             Following Sensors (manufactured after 11/1/07) which have additional
00894             sensor inputs on the sides of the board. This can be used to read
00895             additional Single Line Following sensors, or read any type of on/off
00896             momentary switch, such those used for bumpers.
00897         '''
00898         cmd = ('line7 %d' if seven else 'line %d') %addr
00899         if newaddr: cmd += ' -a %d' %newaddr
00900         return self.execute_array(cmd)
00901 
00902     def i2c(self, op, addr, data=None):
00903         ''' Usage1: i2c(op, addr)
00904             Usage2: i2c(op, addr, data)
00905             The flexible i2c command allows you to execute a generic i2c read, or
00906             write command to the specified device at address addr. Depending on
00907             whether you issue a read or write, additional parameters vary.
00908         '''
00909         cmd = 'i2c %c 0x%X ' %(op, addr)
00910         if data:
00911             cmd += ''.join(data)
00912         if op == 'w':
00913             return self.execute_ack(cmd)
00914         else:
00915             return self.execute_array(cmd)
00916         
00917     def voltage(self, cached=False):
00918         if cached:
00919             try:
00920                 value = self.analog_sensor_cache[5]
00921             except:
00922                 try:
00923                     value = self.sensor(5)
00924                     self.update_analog_cache(5, value)
00925                 except:
00926                     return None
00927         else:
00928             try:
00929                 value = self.sensor(5)
00930                 self.update_analog_cache(5, value)
00931             except:
00932                 return None
00933         
00934         return value * 15. / 1024.
00935         
00936     def set_wheel_diameter(self, diameter):
00937         self.wheel_diameter = diameter
00938         
00939     def get_wheel_diameter(self):
00940         return self.wheel_diameter
00941     
00942     def set_wheel_track(self, track):
00943         self.wheel_track = track
00944         
00945     def get_wheel_track(self):
00946         return self.wheel_track
00947     
00948     def set_gear_reduction(self, ratio):
00949         self.gear_reduction = ratio
00950         
00951     def get_gear_reduction(self):
00952         return self.gear_reduction
00953     
00954     def set_encoder_resolution(self, ticks):
00955         self.encoder_resolution = ticks
00956         
00957     def get_encoder_resolution(self):
00958         return self.encoder_resolution
00959     
00960     def get_Ping(self, pin, cached=False):
00961         ''' Get the distance reading from a Ping sonarl sensor on the given GPIO pin.
00962         '''
00963         if cached:
00964             value = self.digital_sensor_cache[pin]
00965         else:
00966             value = self.pping(pin)
00967         return value
00968     
00969     def get_MaxEZ1(self, trigger_pin, output_pin, cached=False):
00970         ''' Get the distance reading from a MaxEZ1 sonarl sensor on the given trigger and output pins.
00971         '''
00972         if cached:
00973             value = self.digital_sensor_cache[trigger_pin]
00974         else:
00975             value = self.get_maxez1(trigger_pin, output_pin)
00976         return value
00977     
00978     def get_GP2D12(self, pin, cached=False):
00979         ''' Get the distance reading from a GP2D12 IR sensor on the given analog pin.
00980         '''
00981         if cached:
00982             value = self.analog_sensor_cache[pin]
00983         else:
00984             value = self.sensor(pin)
00985         try:
00986             distance = (6787 / (value - 3)) - 4
00987         except:
00988             distance = 80
00989         if distance > 80: distance = 80
00990         if distance < 10: distance = 10   
00991         if self.units == 0:
00992             return distance
00993         elif self.units == 1:
00994             return distance / 2.54
00995         else:
00996             return value
00997         
00998     def get_PhidgetsTemperature(self, pin, cached=False, units="F"):
00999         ''' Get the temperature from a Phidgets Temperature sensor on an analog sensor port and return
01000             the reading in either Farhenheit or Celcius depending on the units argument.
01001         '''
01002         self.temp_units = units
01003         if cached:
01004             value = self.analog_sensor_cache[pin]
01005         else:
01006             value = self.sensor(pin)
01007         try:
01008             #tempC = (value - 200) / 4.0
01009             tempC = (value / 4.0 * 0.2222) - 61.111
01010             #tempC = (200 - value / 4.0) / 4.0
01011             if self.temp_units == "C":
01012                 return tempC
01013             else:
01014                 return 9. * tempC / 5. + 32.
01015         except:
01016             return self.BAD_VALUE
01017         
01018     def get_PhidgetsVoltage(self, pin, cached=False):
01019         ''' Get the voltage from a Phidgets Voltage sensor on an analog sensor port.
01020         '''
01021         if cached:
01022             value = self.analog_sensor_cache[pin]
01023         else:
01024             value = self.sensor(pin)
01025         try:
01026             return 0.06 * (value - 500.)
01027         except:
01028             return self.BAD_VALUE
01029     
01030     def get_PhidgetsCurrent(self, pin, cached=False, model=20, ac_dc="dc"):
01031         if cached:
01032             value = self.analog_sensor_cache[pin]
01033         else:
01034             value = self.sensor(pin)
01035         try:
01036             if model == 20:
01037                 if ac_dc == "dc":
01038                     return 0.05 * (value - 500.)
01039                 else:
01040                     return 0.025 * value
01041             else:
01042                 if ac_dc == "dc":
01043                     return 0.125 * (value - 500.)
01044                 else:
01045                     return 0.625 * value
01046         except:
01047             return self.BAD_VALUE
01048          
01049     def travel_distance(self, dist, vel):
01050         ''' Move forward or backward 'dist' (inches or meters depending on units) at speed 'vel'.  Use negative distances
01051             to move backward.
01052         '''
01053             
01054         revs_per_second = float(vel) / (self.wheel_diameter * math.pi)
01055             
01056         ticks_per_loop = revs_per_second * self.encoder_resolution * self.loop_interval * self.gear_reduction
01057         vel = int(ticks_per_loop)
01058             
01059         revs = dist / (self.wheel_diameter * math.pi)
01060         ticks = revs * self.encoder_resolution * self.gear_reduction
01061         self.digo([1, 2], [ticks, ticks], [vel, vel])
01062         
01063     def mogo(self, id, vel):
01064         ''' Usage 1: mogo(id, vel)
01065             Usage 2: mogo([id1, id2], [vel1, vel2])
01066             The mogo command sets motor speed using one or more complex
01067             parameters containing a <motorId:spd> value pair.
01068             The motorId can be either 1 or 2, which corresponds to the Motor
01069             Terminal port.
01070             The vel value specifies the motor velocity, and it's range depends on
01071             your VPID settings. See the VPID parameters section below to
01072             determine your MAX velocity. A positive value rotates the motors in
01073             one direction, which a negative value rotates the motors in the
01074             opposite direction.
01075             You will have to determine which direction is positive for your motors,
01076             and connect the motors wires to the terminals on the Element board
01077             in the appropriate configuration.
01078         '''
01079         if type(id) == int: id = [id]
01080         if type(vel) == int: vel = [vel]
01081         
01082         if self.motors_reversed:
01083             for i in range(len(vel)):
01084                 vel[i] = -1 * vel[i]
01085                       
01086         return self.execute_ack('mogo %s' %' '.join(map(lambda x: '%d:%d' %x, zip(id, vel))))
01087     
01088     def mogo_m_per_s(self, id, vel):
01089         ''' Set the motor speeds in meters per second.
01090         '''
01091         if type(id) != list: id = [id]
01092         if type(vel) != list: vel = [vel]
01093         spd = list()
01094         for v in vel:
01095             if self.units == 0:
01096                 revs_per_second = float(v) / (self.wheel_diameter * math.pi)
01097             elif self.units == 1:
01098                 revs_per_second = float(v) / (self.wheel_diameter * math.pi * 2.54 / 100)
01099             ticks_per_loop = revs_per_second * self.encoder_resolution * self.loop_interval * self.gear_reduction
01100             spd.append(int(ticks_per_loop))
01101         
01102         if self.motors_reversed:
01103             for i in range(len(spd)):
01104                 spd[i] = -1 * spd[i]
01105 
01106         cmd = 'mogo %s' %' '.join(map(lambda x: '%d:%d' %x, zip(id, spd)))
01107                             
01108         return self.execute_ack(cmd)
01109         
01110     def travel_at_speed(self, id, vel):
01111         ''' Move forward or backward at speed 'vel' in meters per second.  Use negative speeds
01112             to move backward.  This is just an alias for mogo_m_per_s so see that function for
01113             more details.
01114         '''
01115         self.mogo_m_per_s(id, vel)
01116         
01117     def rotate(self, angle, vel):
01118         ''' Rotate the robot through 'angle' degrees or radians at speed 'vel'.  Use the ROS convention for right handed
01119             coordinate systems with the z-axis pointing up so that a positive angle is counterclockwise.
01120             Use negative angles to rotate clockwise.
01121         '''        
01122         if self.units == 0:
01123             revs_per_second = float(vel) / (2.0 * math.pi)
01124             rotation_fraction = angle / (2.0 * math.pi)
01125             full_rotation_dist = self.wheel_track * math.pi
01126         elif self.units == 1:
01127             revs_per_second = float(vel) / 360.
01128             rotation_fraction = angle / 360.
01129             full_rotation_dist = self.wheel_track * 2.54 / 100 * math.pi
01130 
01131         vel = revs_per_second * full_rotation_dist # in m/s
01132 
01133         rotation_dist = rotation_fraction * full_rotation_dist
01134         revs = rotation_dist / (self.wheel_diameter * math.pi)
01135 
01136         ticks = revs * self.encoder_resolution  * self.gear_reduction
01137         self.digo_m_per_s([1, 2], [ticks, -ticks], [vel, vel])
01138         
01139     def rotate_at_speed(self, vel):
01140         ''' Rotate the robot continously at speed 'vel' radians or degrees per second.  Use the ROS convention for right handed
01141             coordinate systems with the z-axis pointing up so that a positive speed is counterclockwise.
01142             Use negative speeds to rotate clockwise.
01143         '''
01144             
01145         if self.units == 1:
01146             vel = vel / 180. * math.pi
01147             
01148         # Check that the user does not mistaken degrees for radians.
01149         if vel > 5.0:
01150             print "That is a rather high rotation rate. Are you sure you specified rotation velocity in the correct units?"
01151             print "Degrees per second for English units and radians per second for metric."
01152             print "Keep in mind that 1 radian per second is about 60 degrees per second."
01153             os._exit(1)
01154             
01155         if self.units == 1:
01156             full_rotation_dist = self.wheel_track * 2.54 / 100 * math.pi
01157             revs_per_second = float(vel) / 360.
01158 
01159         else:
01160             full_rotation_dist = self.wheel_track * math.pi
01161             revs_per_second = float(vel) / (2.0 * math.pi) 
01162                
01163         vel = revs_per_second * full_rotation_dist
01164 
01165         self.mogo_m_per_s([1, 2], [-vel, vel])
01166         
01167     def twist(self, twist):
01168         angle = math.atan2(twist.linear.y, twist.linear.x)
01169         self.rotate(angle, twist.angular.z)
01170         while self.get_pids():
01171             time.sleep(0.05)
01172         speed = math.sqrt(twist.linear.x * twist.linear.x + twist.linear.y * twist.linear.y)
01173         self.mogo_m_per_s([1, 2], [speed, speed])
01174     
01175 class Linear:
01176     def __init__(self):
01177         self.x = 0
01178         self.y = 0
01179         self.z = 0
01180         
01181 class Angular:
01182     def __init__(self):
01183         self.x = 0
01184         self.y = 0
01185         self.z = 0
01186         
01187 class Twist:
01188     def __init__(self):
01189         self.linear = Linear()
01190         self.angular = Angular()      
01191 
01192 class PhidgetsTemperature():
01193     def __init__(self, Element, pin, units="F"):
01194         ''' Usage: myTemp = PhidgetsTemperature(Element, pin)
01195                    reading = myTemp.value()
01196                    reading = myTemp.value(cached=True) # gets value from the cache
01197             The Phidgets Temperature Sensor class wraps an analog sensor port and converts the raw
01198             sensor reading to either Farhenheit or Celcius depending on the units argument.
01199         '''
01200         self.Element = Element
01201         self.pin = pin
01202         self.temp_units = units
01203     
01204     def value(self, cached=False, units="F"):   
01205         return self.Element.get_PhidgetsTemperature(self.pin, cached, units)
01206     
01207 
01208 class PhidgetsVoltage():
01209     def __init__(self, Element, pin):
01210         ''' Usage: myVolts = PhidgetsVoltage(Element, pin)
01211                    reading = myVolts.value()
01212                    reading = myVolts.value(cached=True) # gets value from the cache
01213             The Phidgets Voltage Sensor class wraps an analog sensor port and converts the raw
01214             sensor reading to volts.
01215         '''
01216         self.Element = Element
01217         self.pin = pin
01218     
01219     def value(self, cached=False):
01220         return self.Element.get_PhidgetsVoltage(self.pin, cached)
01221  
01222 
01223 class PhidgetsCurrent():
01224     def __init__(self, Element, pin, model=20, ac_dc="dc"):
01225         ''' Usage: myAmps = PhidgetsCurrent(Element, pin)
01226                    reading = myAmps.value()
01227                    reading = myAmps.value(cached=True) # gets value from the cache
01228             The Phidgets Current Sensor class wraps an analog sensor port and converts the raw
01229             sensor reading to amps.  Note there are two models of the Phidgets current sensor, 
01230             one with a 20 amp max and the other with a 50 amp max. Also, either model can 
01231             measure either DC or AC current.
01232         '''
01233         self.Element = Element
01234         self.pin = pin
01235         self.model = model
01236         self.ac_dc = ac_dc
01237         
01238     def value(self, cached=False):
01239         return self.Element.get_PhidgetsCurrent(self.pin, cached, self.model, self.ac_dc)        
01240 
01241 
01242 class Ping():
01243     def __init__(self, Element, pin):
01244         ''' Usage: myPing = Ping(Element, pin)
01245                    reading = myPing.value()
01246                    reading = myPing.value(cached=True) # gets value from the cache
01247             The Parallax Ping Sonar Sensor class wraps a digital sensor port returns the value
01248             from the pping() command which in turn is in inches or cm depending on the units settings.
01249         '''
01250         self.Element = Element
01251         self.pin = pin
01252     
01253     def value(self, cached=False):   
01254         return self.Element.get_Ping(self.pin, cached)
01255     
01256 class GP2D12():
01257     def __init__(self, Element, pin):
01258         ''' Usage: myIR = GP2D12(Element, pin)
01259                    reading = myIR.value()
01260                    reading = myIR.value(cached=True) # gets value from the cache
01261             The Sharp GPD12 IR Sensor class wraps an analog sensor port and converts the raw
01262             sensor reading to either inches or cm depending on the units settings.
01263         '''
01264         self.Element = Element
01265         self.pin = pin
01266 
01267     def value(self, cached=False):   
01268         return self.Element.get_GP2D12(self.pin, cached)
01269  
01270 
01271 """ Basic test for connectivity """
01272 if __name__ == "__main__":
01273     if os.name == "posix":
01274         portName = "/dev/ttyUSB0"
01275         #portName = "/dev/rfcomm0" # For bluetooth on Linux
01276         # Note: On Linux, after connecting to the Bluetooth adapter, run the command
01277         # sudo rfcomm bind /dev/rfcomm0
01278     else:
01279         portName = "COM43" # Windows style COM port.
01280         
01281     baudRate = 57600
01282 
01283     myElement = Element(port=portName, baudrate=baudRate, timeout=0.5, units=0)
01284     myElement.connect()
01285      
01286     print "Sleeping for 2 seconds..."
01287     time.sleep(2)   
01288     
01289     print "Firmware Version", myElement.fw()
01290     print "Units", myElement.get_units()
01291     print "Baudrate", myElement.get_baud()
01292     print "VPID", myElement.get_vpid()
01293     print "DPID", myElement.get_dpid()
01294     print "Voltage", myElement.voltage()
01295     
01296     print "Connection test successful.",
01297     
01298     myElement.stop()
01299     myElement.close()
01300     
01301     print "Shutting down Element."
01302     


element
Author(s): Patrick Goebel
autogenerated on Mon Jan 6 2014 11:16:54