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