Matthew Phillipps / PololuQTRSensors

Pololu QTR Sensor Library, based on the QTR Arduino Library

Dependents:   Nucleo_QTR ZumoReflectanceSensorArray speed_robot

QTRSensors.cpp@0:d54bb6a949bf, 2015-08-25 (annotated)

Committer:
phillippsm
Date:
Tue Aug 25 02:44:57 2015 +0000
Revision:
0:d54bb6a949bf
Child:
1:a664ab7aba8d
Alpha Release

Who changed what in which revision?

UserRevisionLine numberNew contents of line
phillippsm 0:d54bb6a949bf 1 /*
phillippsm 0:d54bb6a949bf 2 QTRSensors.cpp - Arduino library for using Pololu QTR reflectance
phillippsm 0:d54bb6a949bf 3 sensors and reflectance sensor arrays: QTR-1A, QTR-8A, QTR-1RC, and
phillippsm 0:d54bb6a949bf 4 QTR-8RC. The object used will determine the type of the sensor (either
phillippsm 0:d54bb6a949bf 5 QTR-xA or QTR-xRC). Then simply specify in the constructor which
phillippsm 0:d54bb6a949bf 6 Arduino I/O pins are connected to a QTR sensor, and the read() method
phillippsm 0:d54bb6a949bf 7 will obtain reflectance measurements for those sensors. Smaller sensor
phillippsm 0:d54bb6a949bf 8 values correspond to higher reflectance (e.g. white) while larger
phillippsm 0:d54bb6a949bf 9 sensor values correspond to lower reflectance (e.g. black or a void).
phillippsm 0:d54bb6a949bf 10
phillippsm 0:d54bb6a949bf 11 * QTRSensorsRC should be used for QTR-1RC and QTR-8RC sensors.
phillippsm 0:d54bb6a949bf 12 * QTRSensorsAnalog should be used for QTR-1A and QTR-8A sensors.
phillippsm 0:d54bb6a949bf 13 */
phillippsm 0:d54bb6a949bf 14
phillippsm 0:d54bb6a949bf 15 /*
phillippsm 0:d54bb6a949bf 16 * Written by Ben Schmidel et al., October 4, 2010.
phillippsm 0:d54bb6a949bf 17 * Copyright (c) 2008-2012 Pololu Corporation. For more information, see
phillippsm 0:d54bb6a949bf 18 *
phillippsm 0:d54bb6a949bf 19 * http://www.pololu.com
phillippsm 0:d54bb6a949bf 20 * http://forum.pololu.com
phillippsm 0:d54bb6a949bf 21 * http://www.pololu.com/docs/0J19
phillippsm 0:d54bb6a949bf 22 *
phillippsm 0:d54bb6a949bf 23 * You may freely modify and share this code, as long as you keep this
phillippsm 0:d54bb6a949bf 24 * notice intact (including the two links above). Licensed under the
phillippsm 0:d54bb6a949bf 25 * Creative Commons BY-SA 3.0 license:
phillippsm 0:d54bb6a949bf 26 *
phillippsm 0:d54bb6a949bf 27 * http://creativecommons.org/licenses/by-sa/3.0/
phillippsm 0:d54bb6a949bf 28 *
phillippsm 0:d54bb6a949bf 29 * Disclaimer: To the extent permitted by law, Pololu provides this work
phillippsm 0:d54bb6a949bf 30 * without any warranty. It might be defective, in which case you agree
phillippsm 0:d54bb6a949bf 31 * to be responsible for all resulting costs and damages.
phillippsm 0:d54bb6a949bf 32 *
phillippsm 0:d54bb6a949bf 33 * Modified by Matthew Phillipps, August 24, 2015
phillippsm 0:d54bb6a949bf 34 * Adapted to mbed platform (especially STM Nucleo boards)
phillippsm 0:d54bb6a949bf 35 * Some changes to memory management
phillippsm 0:d54bb6a949bf 36 */
phillippsm 0:d54bb6a949bf 37
phillippsm 0:d54bb6a949bf 38 #include <stdlib.h>
phillippsm 0:d54bb6a949bf 39 #include "QTRSensors.h"
phillippsm 0:d54bb6a949bf 40 #include "mbed.h"
phillippsm 0:d54bb6a949bf 41
phillippsm 0:d54bb6a949bf 42
phillippsm 0:d54bb6a949bf 43 Timer timer;
phillippsm 0:d54bb6a949bf 44
phillippsm 0:d54bb6a949bf 45
phillippsm 0:d54bb6a949bf 46 // Base class data member initialization (called by derived class init())
phillippsm 0:d54bb6a949bf 47 void QTRSensors::init(PinName *pins, unsigned char numSensors,
phillippsm 0:d54bb6a949bf 48 PinName emitterPin, bool analog = false)
phillippsm 0:d54bb6a949bf 49 {
phillippsm 0:d54bb6a949bf 50 calibratedMinimumOn=0;
phillippsm 0:d54bb6a949bf 51 calibratedMaximumOn=0;
phillippsm 0:d54bb6a949bf 52 calibratedMinimumOff=0;
phillippsm 0:d54bb6a949bf 53 calibratedMaximumOff=0;
phillippsm 0:d54bb6a949bf 54
phillippsm 0:d54bb6a949bf 55 if (numSensors > QTR_MAX_SENSORS)
phillippsm 0:d54bb6a949bf 56 _numSensors = QTR_MAX_SENSORS;
phillippsm 0:d54bb6a949bf 57 else
phillippsm 0:d54bb6a949bf 58 _numSensors = numSensors;
phillippsm 0:d54bb6a949bf 59
phillippsm 0:d54bb6a949bf 60
phillippsm 0:d54bb6a949bf 61 if (_pins == 0)
phillippsm 0:d54bb6a949bf 62 {
phillippsm 0:d54bb6a949bf 63 _pins = (PinName *)malloc(sizeof(PinName)*_numSensors);
phillippsm 0:d54bb6a949bf 64 if (_pins == 0)
phillippsm 0:d54bb6a949bf 65 return;
phillippsm 0:d54bb6a949bf 66 }
phillippsm 0:d54bb6a949bf 67 unsigned char i;
phillippsm 0:d54bb6a949bf 68
phillippsm 0:d54bb6a949bf 69 // Allocate the arrays
phillippsm 0:d54bb6a949bf 70 if(calibratedMaximumOn == 0)
phillippsm 0:d54bb6a949bf 71 {
phillippsm 0:d54bb6a949bf 72 calibratedMaximumOn = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
phillippsm 0:d54bb6a949bf 73
phillippsm 0:d54bb6a949bf 74 // If the malloc failed, don't continue.
phillippsm 0:d54bb6a949bf 75 if(calibratedMaximumOn == 0)
phillippsm 0:d54bb6a949bf 76 return;
phillippsm 0:d54bb6a949bf 77
phillippsm 0:d54bb6a949bf 78 // Initialize the max and min calibrated values to values that
phillippsm 0:d54bb6a949bf 79 // will cause the first reading to update them.
phillippsm 0:d54bb6a949bf 80
phillippsm 0:d54bb6a949bf 81 for(i=0;i<_numSensors;i++)
phillippsm 0:d54bb6a949bf 82 calibratedMaximumOn[i] = 0;
phillippsm 0:d54bb6a949bf 83 }
phillippsm 0:d54bb6a949bf 84 if(calibratedMaximumOff == 0)
phillippsm 0:d54bb6a949bf 85 {
phillippsm 0:d54bb6a949bf 86 calibratedMaximumOff = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
phillippsm 0:d54bb6a949bf 87
phillippsm 0:d54bb6a949bf 88 // If the malloc failed, don't continue.
phillippsm 0:d54bb6a949bf 89 if(calibratedMaximumOff == 0)
phillippsm 0:d54bb6a949bf 90 return;
phillippsm 0:d54bb6a949bf 91
phillippsm 0:d54bb6a949bf 92 // Initialize the max and min calibrated values to values that
phillippsm 0:d54bb6a949bf 93 // will cause the first reading to update them.
phillippsm 0:d54bb6a949bf 94
phillippsm 0:d54bb6a949bf 95 for(i=0;i<_numSensors;i++)
phillippsm 0:d54bb6a949bf 96 calibratedMaximumOff[i] = 0;
phillippsm 0:d54bb6a949bf 97 }
phillippsm 0:d54bb6a949bf 98 if(calibratedMinimumOn == 0)
phillippsm 0:d54bb6a949bf 99 {
phillippsm 0:d54bb6a949bf 100 calibratedMinimumOn = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
phillippsm 0:d54bb6a949bf 101
phillippsm 0:d54bb6a949bf 102 // If the malloc failed, don't continue.
phillippsm 0:d54bb6a949bf 103 if(calibratedMinimumOn == 0)
phillippsm 0:d54bb6a949bf 104 return;
phillippsm 0:d54bb6a949bf 105
phillippsm 0:d54bb6a949bf 106 for(i=0;i<_numSensors;i++)
phillippsm 0:d54bb6a949bf 107 calibratedMinimumOn[i] = _maxValue;
phillippsm 0:d54bb6a949bf 108 }
phillippsm 0:d54bb6a949bf 109 if(calibratedMinimumOff == 0)
phillippsm 0:d54bb6a949bf 110 {
phillippsm 0:d54bb6a949bf 111 calibratedMinimumOff = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
phillippsm 0:d54bb6a949bf 112
phillippsm 0:d54bb6a949bf 113 // If the malloc failed, don't continue.
phillippsm 0:d54bb6a949bf 114 if(calibratedMinimumOff == 0)
phillippsm 0:d54bb6a949bf 115 return;
phillippsm 0:d54bb6a949bf 116
phillippsm 0:d54bb6a949bf 117 for(i=0;i<_numSensors;i++)
phillippsm 0:d54bb6a949bf 118 calibratedMinimumOff[i] = _maxValue;
phillippsm 0:d54bb6a949bf 119 }
phillippsm 0:d54bb6a949bf 120
phillippsm 0:d54bb6a949bf 121
phillippsm 0:d54bb6a949bf 122
phillippsm 0:d54bb6a949bf 123 for (i = 0; i < _numSensors; i++)
phillippsm 0:d54bb6a949bf 124 {
phillippsm 0:d54bb6a949bf 125 _pins[i] = pins[i];
phillippsm 0:d54bb6a949bf 126 }
phillippsm 0:d54bb6a949bf 127
phillippsm 0:d54bb6a949bf 128 _emitterPin = emitterPin;
phillippsm 0:d54bb6a949bf 129 _emitter = new DigitalOut(emitterPin);
phillippsm 0:d54bb6a949bf 130
phillippsm 0:d54bb6a949bf 131 _analog = analog;
phillippsm 0:d54bb6a949bf 132 if (_analog) {
phillippsm 0:d54bb6a949bf 133 _qtrAIPins.reserve(_numSensors);
phillippsm 0:d54bb6a949bf 134 } else {
phillippsm 0:d54bb6a949bf 135 _qtrPins.reserve(_numSensors);
phillippsm 0:d54bb6a949bf 136 }
phillippsm 0:d54bb6a949bf 137 for (i = 0; i < _numSensors; i++)
phillippsm 0:d54bb6a949bf 138 {
phillippsm 0:d54bb6a949bf 139 if (_analog) {
phillippsm 0:d54bb6a949bf 140 _qtrAIPins.push_back(new AnalogIn(pins[i]));
phillippsm 0:d54bb6a949bf 141 } else {
phillippsm 0:d54bb6a949bf 142 _qtrPins.push_back(new DigitalInOut(pins[i]));
phillippsm 0:d54bb6a949bf 143 }
phillippsm 0:d54bb6a949bf 144 }
phillippsm 0:d54bb6a949bf 145
phillippsm 0:d54bb6a949bf 146 }
phillippsm 0:d54bb6a949bf 147
phillippsm 0:d54bb6a949bf 148
phillippsm 0:d54bb6a949bf 149 // Reads the sensor values into an array. There *MUST* be space
phillippsm 0:d54bb6a949bf 150 // for as many values as there were sensors specified in the constructor.
phillippsm 0:d54bb6a949bf 151 // Example usage:
phillippsm 0:d54bb6a949bf 152 // unsigned int sensor_values[8];
phillippsm 0:d54bb6a949bf 153 // sensors.read(sensor_values);
phillippsm 0:d54bb6a949bf 154 // The values returned are a measure of the reflectance in abstract units,
phillippsm 0:d54bb6a949bf 155 // with higher values corresponding to lower reflectance (e.g. a black
phillippsm 0:d54bb6a949bf 156 // surface or a void).
phillippsm 0:d54bb6a949bf 157 void QTRSensors::read(unsigned int *sensor_values, unsigned char readMode)
phillippsm 0:d54bb6a949bf 158 {
phillippsm 0:d54bb6a949bf 159 unsigned int off_values[QTR_MAX_SENSORS];
phillippsm 0:d54bb6a949bf 160 unsigned char i;
phillippsm 0:d54bb6a949bf 161
phillippsm 0:d54bb6a949bf 162
phillippsm 0:d54bb6a949bf 163 if(readMode == QTR_EMITTERS_ON || readMode == QTR_EMITTERS_ON_AND_OFF)
phillippsm 0:d54bb6a949bf 164 {
phillippsm 0:d54bb6a949bf 165 emittersOn(); }
phillippsm 0:d54bb6a949bf 166 else
phillippsm 0:d54bb6a949bf 167 {
phillippsm 0:d54bb6a949bf 168 emittersOff();
phillippsm 0:d54bb6a949bf 169 }
phillippsm 0:d54bb6a949bf 170
phillippsm 0:d54bb6a949bf 171
phillippsm 0:d54bb6a949bf 172 readPrivate(sensor_values);
phillippsm 0:d54bb6a949bf 173
phillippsm 0:d54bb6a949bf 174 emittersOff();
phillippsm 0:d54bb6a949bf 175
phillippsm 0:d54bb6a949bf 176 if(readMode == QTR_EMITTERS_ON_AND_OFF)
phillippsm 0:d54bb6a949bf 177 {
phillippsm 0:d54bb6a949bf 178 readPrivate(off_values);
phillippsm 0:d54bb6a949bf 179
phillippsm 0:d54bb6a949bf 180 for(i=0;i<_numSensors;i++)
phillippsm 0:d54bb6a949bf 181 {
phillippsm 0:d54bb6a949bf 182 sensor_values[i] += _maxValue - off_values[i];
phillippsm 0:d54bb6a949bf 183 }
phillippsm 0:d54bb6a949bf 184 }
phillippsm 0:d54bb6a949bf 185 }
phillippsm 0:d54bb6a949bf 186
phillippsm 0:d54bb6a949bf 187
phillippsm 0:d54bb6a949bf 188 // Turn the IR LEDs off and on. This is mainly for use by the
phillippsm 0:d54bb6a949bf 189 // read method, and calling these functions before or
phillippsm 0:d54bb6a949bf 190 // after the reading the sensors will have no effect on the
phillippsm 0:d54bb6a949bf 191 // readings, but you may wish to use these for testing purposes.
phillippsm 0:d54bb6a949bf 192 void QTRSensors::emittersOff()
phillippsm 0:d54bb6a949bf 193 {
phillippsm 0:d54bb6a949bf 194 if (_emitterPin == QTR_NO_EMITTER_PIN)
phillippsm 0:d54bb6a949bf 195 return;
phillippsm 0:d54bb6a949bf 196 // pinMode(_emitterPin, OUTPUT);
phillippsm 0:d54bb6a949bf 197 // digitalWrite(_emitterPin, LOW);
phillippsm 0:d54bb6a949bf 198 _emitter->write(LOW); // 0 is low
phillippsm 0:d54bb6a949bf 199 // delayMicroseconds(200);
phillippsm 0:d54bb6a949bf 200 wait_ms(20); //200 was too long
phillippsm 0:d54bb6a949bf 201
phillippsm 0:d54bb6a949bf 202 }
phillippsm 0:d54bb6a949bf 203
phillippsm 0:d54bb6a949bf 204 void QTRSensors::emittersOn()
phillippsm 0:d54bb6a949bf 205 {
phillippsm 0:d54bb6a949bf 206 if (_emitterPin == QTR_NO_EMITTER_PIN)
phillippsm 0:d54bb6a949bf 207 return;
phillippsm 0:d54bb6a949bf 208
phillippsm 0:d54bb6a949bf 209 //pinMode(_emitterPin, OUTPUT);
phillippsm 0:d54bb6a949bf 210 //digitalWrite(_emitterPin, HIGH);
phillippsm 0:d54bb6a949bf 211 _emitter->write(HIGH);
phillippsm 0:d54bb6a949bf 212 //delayMicroseconds(200);
phillippsm 0:d54bb6a949bf 213 wait_ms(20); // 200 was too long
phillippsm 0:d54bb6a949bf 214 }
phillippsm 0:d54bb6a949bf 215
phillippsm 0:d54bb6a949bf 216 // Resets the calibration.
phillippsm 0:d54bb6a949bf 217 void QTRSensors::resetCalibration()
phillippsm 0:d54bb6a949bf 218 {
phillippsm 0:d54bb6a949bf 219 unsigned char i;
phillippsm 0:d54bb6a949bf 220 for(i=0;i<_numSensors;i++)
phillippsm 0:d54bb6a949bf 221 {
phillippsm 0:d54bb6a949bf 222 if(calibratedMinimumOn)
phillippsm 0:d54bb6a949bf 223 calibratedMinimumOn[i] = _maxValue;
phillippsm 0:d54bb6a949bf 224 if(calibratedMinimumOff)
phillippsm 0:d54bb6a949bf 225 calibratedMinimumOff[i] = _maxValue;
phillippsm 0:d54bb6a949bf 226 if(calibratedMaximumOn)
phillippsm 0:d54bb6a949bf 227 calibratedMaximumOn[i] = 0;
phillippsm 0:d54bb6a949bf 228 if(calibratedMaximumOff)
phillippsm 0:d54bb6a949bf 229 calibratedMaximumOff[i] = 0;
phillippsm 0:d54bb6a949bf 230 }
phillippsm 0:d54bb6a949bf 231 }
phillippsm 0:d54bb6a949bf 232
phillippsm 0:d54bb6a949bf 233 // Reads the sensors 10 times and uses the results for
phillippsm 0:d54bb6a949bf 234 // calibration. The sensor values are not returned; instead, the
phillippsm 0:d54bb6a949bf 235 // maximum and minimum values found over time are stored internally
phillippsm 0:d54bb6a949bf 236 // and used for the readCalibrated() method.
phillippsm 0:d54bb6a949bf 237 void QTRSensors::calibrate(unsigned char readMode)
phillippsm 0:d54bb6a949bf 238 {
phillippsm 0:d54bb6a949bf 239 if(readMode == QTR_EMITTERS_ON_AND_OFF || readMode == QTR_EMITTERS_ON)
phillippsm 0:d54bb6a949bf 240 {
phillippsm 0:d54bb6a949bf 241 calibrateOnOrOff(&calibratedMinimumOn,
phillippsm 0:d54bb6a949bf 242 &calibratedMaximumOn,
phillippsm 0:d54bb6a949bf 243 QTR_EMITTERS_ON);
phillippsm 0:d54bb6a949bf 244 }
phillippsm 0:d54bb6a949bf 245
phillippsm 0:d54bb6a949bf 246
phillippsm 0:d54bb6a949bf 247 if(readMode == QTR_EMITTERS_ON_AND_OFF || readMode == QTR_EMITTERS_OFF)
phillippsm 0:d54bb6a949bf 248 {
phillippsm 0:d54bb6a949bf 249 calibrateOnOrOff(&calibratedMinimumOff,
phillippsm 0:d54bb6a949bf 250 &calibratedMaximumOff,
phillippsm 0:d54bb6a949bf 251 QTR_EMITTERS_OFF);
phillippsm 0:d54bb6a949bf 252 }
phillippsm 0:d54bb6a949bf 253 }
phillippsm 0:d54bb6a949bf 254
phillippsm 0:d54bb6a949bf 255 void QTRSensors::calibrateOnOrOff(unsigned int **calibratedMinimum,
phillippsm 0:d54bb6a949bf 256 unsigned int **calibratedMaximum,
phillippsm 0:d54bb6a949bf 257 unsigned char readMode)
phillippsm 0:d54bb6a949bf 258 {
phillippsm 0:d54bb6a949bf 259 int i;
phillippsm 0:d54bb6a949bf 260 unsigned int sensor_values[16];
phillippsm 0:d54bb6a949bf 261 unsigned int max_sensor_values[16];
phillippsm 0:d54bb6a949bf 262 unsigned int min_sensor_values[16];
phillippsm 0:d54bb6a949bf 263 for(i=0;i<_numSensors;i++) {
phillippsm 0:d54bb6a949bf 264 sensor_values[i] = 0;
phillippsm 0:d54bb6a949bf 265 max_sensor_values[i] = 0;
phillippsm 0:d54bb6a949bf 266 min_sensor_values[i] = _maxValue;
phillippsm 0:d54bb6a949bf 267 }
phillippsm 0:d54bb6a949bf 268
phillippsm 0:d54bb6a949bf 269 int j;
phillippsm 0:d54bb6a949bf 270 for(j=0;j<10;j++)
phillippsm 0:d54bb6a949bf 271 {
phillippsm 0:d54bb6a949bf 272 read(sensor_values,readMode);
phillippsm 0:d54bb6a949bf 273 for(i=0;i<_numSensors;i++)
phillippsm 0:d54bb6a949bf 274 {
phillippsm 0:d54bb6a949bf 275 // set the max we found THIS time
phillippsm 0:d54bb6a949bf 276 if(j == 0 || max_sensor_values[i] < sensor_values[i])
phillippsm 0:d54bb6a949bf 277 max_sensor_values[i] = sensor_values[i];
phillippsm 0:d54bb6a949bf 278
phillippsm 0:d54bb6a949bf 279 // set the min we found THIS time
phillippsm 0:d54bb6a949bf 280 if(j == 0 || min_sensor_values[i] > sensor_values[i])
phillippsm 0:d54bb6a949bf 281 min_sensor_values[i] = sensor_values[i];
phillippsm 0:d54bb6a949bf 282 }
phillippsm 0:d54bb6a949bf 283 }
phillippsm 0:d54bb6a949bf 284
phillippsm 0:d54bb6a949bf 285 // record the min and max calibration values
phillippsm 0:d54bb6a949bf 286 for(i=0;i<_numSensors;i++)
phillippsm 0:d54bb6a949bf 287 {
phillippsm 0:d54bb6a949bf 288 if(min_sensor_values[i] > (*calibratedMaximum)[i]) // this was min_sensor_values[i] > (*calibratedMaximum)[i]
phillippsm 0:d54bb6a949bf 289 (*calibratedMaximum)[i] = min_sensor_values[i];
phillippsm 0:d54bb6a949bf 290 if(max_sensor_values[i] < (*calibratedMinimum)[i])
phillippsm 0:d54bb6a949bf 291 (*calibratedMinimum)[i] = max_sensor_values[i];
phillippsm 0:d54bb6a949bf 292 }
phillippsm 0:d54bb6a949bf 293
phillippsm 0:d54bb6a949bf 294 }
phillippsm 0:d54bb6a949bf 295
phillippsm 0:d54bb6a949bf 296
phillippsm 0:d54bb6a949bf 297 // Returns values calibrated to a value between 0 and 1000, where
phillippsm 0:d54bb6a949bf 298 // 0 corresponds to the minimum value read by calibrate() and 1000
phillippsm 0:d54bb6a949bf 299 // corresponds to the maximum value. Calibration values are
phillippsm 0:d54bb6a949bf 300 // stored separately for each sensor, so that differences in the
phillippsm 0:d54bb6a949bf 301 // sensors are accounted for automatically.
phillippsm 0:d54bb6a949bf 302 void QTRSensors::readCalibrated(unsigned int *sensor_values, unsigned char readMode)
phillippsm 0:d54bb6a949bf 303 {
phillippsm 0:d54bb6a949bf 304 int i;
phillippsm 0:d54bb6a949bf 305
phillippsm 0:d54bb6a949bf 306 // if not calibrated, do nothing
phillippsm 0:d54bb6a949bf 307 if(readMode == QTR_EMITTERS_ON_AND_OFF || readMode == QTR_EMITTERS_OFF)
phillippsm 0:d54bb6a949bf 308 if(!calibratedMinimumOff || !calibratedMaximumOff)
phillippsm 0:d54bb6a949bf 309 return;
phillippsm 0:d54bb6a949bf 310 if(readMode == QTR_EMITTERS_ON_AND_OFF || readMode == QTR_EMITTERS_ON)
phillippsm 0:d54bb6a949bf 311 if(!calibratedMinimumOn || !calibratedMaximumOn)
phillippsm 0:d54bb6a949bf 312 return;
phillippsm 0:d54bb6a949bf 313
phillippsm 0:d54bb6a949bf 314 // read the needed values
phillippsm 0:d54bb6a949bf 315 read(sensor_values,readMode);
phillippsm 0:d54bb6a949bf 316
phillippsm 0:d54bb6a949bf 317 for(i=0;i<_numSensors;i++)
phillippsm 0:d54bb6a949bf 318 {
phillippsm 0:d54bb6a949bf 319 unsigned int calmin,calmax;
phillippsm 0:d54bb6a949bf 320 unsigned int denominator;
phillippsm 0:d54bb6a949bf 321
phillippsm 0:d54bb6a949bf 322 // find the correct calibration
phillippsm 0:d54bb6a949bf 323 if(readMode == QTR_EMITTERS_ON)
phillippsm 0:d54bb6a949bf 324 {
phillippsm 0:d54bb6a949bf 325 calmax = calibratedMaximumOn[i];
phillippsm 0:d54bb6a949bf 326 calmin = calibratedMinimumOn[i];
phillippsm 0:d54bb6a949bf 327 }
phillippsm 0:d54bb6a949bf 328 else if(readMode == QTR_EMITTERS_OFF)
phillippsm 0:d54bb6a949bf 329 {
phillippsm 0:d54bb6a949bf 330 calmax = calibratedMaximumOff[i];
phillippsm 0:d54bb6a949bf 331 calmin = calibratedMinimumOff[i];
phillippsm 0:d54bb6a949bf 332 }
phillippsm 0:d54bb6a949bf 333 else // QTR_EMITTERS_ON_AND_OFF
phillippsm 0:d54bb6a949bf 334 {
phillippsm 0:d54bb6a949bf 335
phillippsm 0:d54bb6a949bf 336 if(calibratedMinimumOff[i] < calibratedMinimumOn[i]) // no meaningful signal
phillippsm 0:d54bb6a949bf 337 calmin = _maxValue;
phillippsm 0:d54bb6a949bf 338 else
phillippsm 0:d54bb6a949bf 339 calmin = calibratedMinimumOn[i] + _maxValue - calibratedMinimumOff[i]; // this won't go past _maxValue
phillippsm 0:d54bb6a949bf 340
phillippsm 0:d54bb6a949bf 341 if(calibratedMaximumOff[i] < calibratedMaximumOn[i]) // no meaningful signal
phillippsm 0:d54bb6a949bf 342 calmax = _maxValue;
phillippsm 0:d54bb6a949bf 343 else
phillippsm 0:d54bb6a949bf 344 calmax = calibratedMaximumOn[i] + _maxValue - calibratedMaximumOff[i]; // this won't go past _maxValue
phillippsm 0:d54bb6a949bf 345 }
phillippsm 0:d54bb6a949bf 346
phillippsm 0:d54bb6a949bf 347 denominator = calmax - calmin;
phillippsm 0:d54bb6a949bf 348
phillippsm 0:d54bb6a949bf 349 signed int x = 0;
phillippsm 0:d54bb6a949bf 350 if(denominator != 0)
phillippsm 0:d54bb6a949bf 351 x = (((signed long)sensor_values[i]) - calmin)
phillippsm 0:d54bb6a949bf 352 * 1000 / denominator;
phillippsm 0:d54bb6a949bf 353 if(x < 0)
phillippsm 0:d54bb6a949bf 354 x = 0;
phillippsm 0:d54bb6a949bf 355 else if(x > 1000)
phillippsm 0:d54bb6a949bf 356 x = 1000;
phillippsm 0:d54bb6a949bf 357 sensor_values[i] = x;
phillippsm 0:d54bb6a949bf 358 }
phillippsm 0:d54bb6a949bf 359
phillippsm 0:d54bb6a949bf 360 }
phillippsm 0:d54bb6a949bf 361
phillippsm 0:d54bb6a949bf 362
phillippsm 0:d54bb6a949bf 363 // Operates the same as read calibrated, but also returns an
phillippsm 0:d54bb6a949bf 364 // estimated position of the robot with respect to a line. The
phillippsm 0:d54bb6a949bf 365 // estimate is made using a weighted average of the sensor indices
phillippsm 0:d54bb6a949bf 366 // multiplied by 1000, so that a return value of 0 indicates that
phillippsm 0:d54bb6a949bf 367 // the line is directly below sensor 0, a return value of 1000
phillippsm 0:d54bb6a949bf 368 // indicates that the line is directly below sensor 1, 2000
phillippsm 0:d54bb6a949bf 369 // indicates that it's below sensor 2000, etc. Intermediate
phillippsm 0:d54bb6a949bf 370 // values indicate that the line is between two sensors. The
phillippsm 0:d54bb6a949bf 371 // formula is:
phillippsm 0:d54bb6a949bf 372 //
phillippsm 0:d54bb6a949bf 373 // 0*value0 + 1000*value1 + 2000*value2 + ...
phillippsm 0:d54bb6a949bf 374 // --------------------------------------------
phillippsm 0:d54bb6a949bf 375 // value0 + value1 + value2 + ...
phillippsm 0:d54bb6a949bf 376 //
phillippsm 0:d54bb6a949bf 377 // By default, this function assumes a dark line (high values)
phillippsm 0:d54bb6a949bf 378 // surrounded by white (low values). If your line is light on
phillippsm 0:d54bb6a949bf 379 // black, set the optional second argument white_line to true. In
phillippsm 0:d54bb6a949bf 380 // this case, each sensor value will be replaced by (1000-value)
phillippsm 0:d54bb6a949bf 381 // before the averaging.
phillippsm 0:d54bb6a949bf 382 int QTRSensors::readLine(unsigned int *sensor_values,
phillippsm 0:d54bb6a949bf 383 unsigned char readMode, unsigned char white_line)
phillippsm 0:d54bb6a949bf 384 {
phillippsm 0:d54bb6a949bf 385 unsigned char i, on_line = 0;
phillippsm 0:d54bb6a949bf 386 unsigned long avg; // this is for the weighted total, which is long
phillippsm 0:d54bb6a949bf 387 // before division
phillippsm 0:d54bb6a949bf 388 unsigned int sum; // this is for the denominator which is <= 64000
phillippsm 0:d54bb6a949bf 389 static int last_value=0; // assume initially that the line is left.
phillippsm 0:d54bb6a949bf 390
phillippsm 0:d54bb6a949bf 391 readCalibrated(sensor_values, readMode);
phillippsm 0:d54bb6a949bf 392
phillippsm 0:d54bb6a949bf 393 avg = 0;
phillippsm 0:d54bb6a949bf 394 sum = 0;
phillippsm 0:d54bb6a949bf 395
phillippsm 0:d54bb6a949bf 396 for(i=0;i<_numSensors;i++) {
phillippsm 0:d54bb6a949bf 397 int value = sensor_values[i];
phillippsm 0:d54bb6a949bf 398 if(white_line)
phillippsm 0:d54bb6a949bf 399 value = 1000-value;
phillippsm 0:d54bb6a949bf 400
phillippsm 0:d54bb6a949bf 401 // keep track of whether we see the line at all
phillippsm 0:d54bb6a949bf 402 if(value > 200) {
phillippsm 0:d54bb6a949bf 403 on_line = 1;
phillippsm 0:d54bb6a949bf 404 }
phillippsm 0:d54bb6a949bf 405
phillippsm 0:d54bb6a949bf 406 // only average in values that are above a noise threshold
phillippsm 0:d54bb6a949bf 407 if(value > 50) {
phillippsm 0:d54bb6a949bf 408 avg += (long)(value) * (i * 1000);
phillippsm 0:d54bb6a949bf 409 sum += value;
phillippsm 0:d54bb6a949bf 410 }
phillippsm 0:d54bb6a949bf 411 }
phillippsm 0:d54bb6a949bf 412
phillippsm 0:d54bb6a949bf 413 if(!on_line)
phillippsm 0:d54bb6a949bf 414 {
phillippsm 0:d54bb6a949bf 415 // If it last read to the left of center, return 0.
phillippsm 0:d54bb6a949bf 416 if(last_value < (_numSensors-1)*1000/2)
phillippsm 0:d54bb6a949bf 417 return 0;
phillippsm 0:d54bb6a949bf 418
phillippsm 0:d54bb6a949bf 419 // If it last read to the right of center, return the max.
phillippsm 0:d54bb6a949bf 420 else
phillippsm 0:d54bb6a949bf 421 return (_numSensors-1)*1000;
phillippsm 0:d54bb6a949bf 422
phillippsm 0:d54bb6a949bf 423 }
phillippsm 0:d54bb6a949bf 424
phillippsm 0:d54bb6a949bf 425 last_value = avg/sum;
phillippsm 0:d54bb6a949bf 426
phillippsm 0:d54bb6a949bf 427 return last_value;
phillippsm 0:d54bb6a949bf 428 }
phillippsm 0:d54bb6a949bf 429
phillippsm 0:d54bb6a949bf 430
phillippsm 0:d54bb6a949bf 431
phillippsm 0:d54bb6a949bf 432 // Derived RC class constructors
phillippsm 0:d54bb6a949bf 433 QTRSensorsRC::QTRSensorsRC()
phillippsm 0:d54bb6a949bf 434 {
phillippsm 0:d54bb6a949bf 435 calibratedMinimumOn = 0;
phillippsm 0:d54bb6a949bf 436 calibratedMaximumOn = 0;
phillippsm 0:d54bb6a949bf 437 calibratedMinimumOff = 0;
phillippsm 0:d54bb6a949bf 438 calibratedMaximumOff = 0;
phillippsm 0:d54bb6a949bf 439 _pins = 0;
phillippsm 0:d54bb6a949bf 440 }
phillippsm 0:d54bb6a949bf 441
phillippsm 0:d54bb6a949bf 442 QTRSensorsRC::QTRSensorsRC(PinName* pins,
phillippsm 0:d54bb6a949bf 443 unsigned char numSensors, unsigned int timeout, PinName emitterPin)
phillippsm 0:d54bb6a949bf 444 {
phillippsm 0:d54bb6a949bf 445 calibratedMinimumOn = 0;
phillippsm 0:d54bb6a949bf 446 calibratedMaximumOn = 0;
phillippsm 0:d54bb6a949bf 447 calibratedMinimumOff = 0;
phillippsm 0:d54bb6a949bf 448 calibratedMaximumOff = 0;
phillippsm 0:d54bb6a949bf 449 _pins = 0;
phillippsm 0:d54bb6a949bf 450
phillippsm 0:d54bb6a949bf 451 init(pins, numSensors, timeout, emitterPin);
phillippsm 0:d54bb6a949bf 452 }
phillippsm 0:d54bb6a949bf 453
phillippsm 0:d54bb6a949bf 454
phillippsm 0:d54bb6a949bf 455 // The array 'pins' contains the Arduino pin number for each sensor.
phillippsm 0:d54bb6a949bf 456
phillippsm 0:d54bb6a949bf 457 // 'numSensors' specifies the length of the 'pins' array (i.e. the
phillippsm 0:d54bb6a949bf 458 // number of QTR-RC sensors you are using). numSensors must be
phillippsm 0:d54bb6a949bf 459 // no greater than 16.
phillippsm 0:d54bb6a949bf 460
phillippsm 0:d54bb6a949bf 461 // 'timeout' specifies the length of time in microseconds beyond
phillippsm 0:d54bb6a949bf 462 // which you consider the sensor reading completely black. That is to say,
phillippsm 0:d54bb6a949bf 463 // if the pulse length for a pin exceeds 'timeout', pulse timing will stop
phillippsm 0:d54bb6a949bf 464 // and the reading for that pin will be considered full black.
phillippsm 0:d54bb6a949bf 465 // It is recommended that you set timeout to be between 1000 and
phillippsm 0:d54bb6a949bf 466 // 3000 us, depending on things like the height of your sensors and
phillippsm 0:d54bb6a949bf 467 // ambient lighting. Using timeout allows you to shorten the
phillippsm 0:d54bb6a949bf 468 // duration of a sensor-reading cycle while still maintaining
phillippsm 0:d54bb6a949bf 469 // useful analog measurements of reflectance
phillippsm 0:d54bb6a949bf 470
phillippsm 0:d54bb6a949bf 471 // 'emitterPin' is the Arduino pin that controls the IR LEDs on the 8RC
phillippsm 0:d54bb6a949bf 472 // modules. If you are using a 1RC (i.e. if there is no emitter pin),
phillippsm 0:d54bb6a949bf 473 // or if you just want the emitters on all the time and don't want to
phillippsm 0:d54bb6a949bf 474 // use an I/O pin to control it, use a value of 255 (QTR_NO_EMITTER_PIN).
phillippsm 0:d54bb6a949bf 475 void QTRSensorsRC::init(PinName* pins,
phillippsm 0:d54bb6a949bf 476 unsigned char numSensors,
phillippsm 0:d54bb6a949bf 477 unsigned int timeout, PinName emitterPin)
phillippsm 0:d54bb6a949bf 478 {
phillippsm 0:d54bb6a949bf 479 QTRSensors::init(pins, numSensors, emitterPin, false);
phillippsm 0:d54bb6a949bf 480
phillippsm 0:d54bb6a949bf 481 _maxValue = timeout;
phillippsm 0:d54bb6a949bf 482 }
phillippsm 0:d54bb6a949bf 483
phillippsm 0:d54bb6a949bf 484
phillippsm 0:d54bb6a949bf 485 // Reads the sensor values into an array. There *MUST* be space
phillippsm 0:d54bb6a949bf 486 // for as many values as there were sensors specified in the constructor.
phillippsm 0:d54bb6a949bf 487 // Example usage:
phillippsm 0:d54bb6a949bf 488 // unsigned int sensor_values[8];
phillippsm 0:d54bb6a949bf 489 // sensors.read(sensor_values);
phillippsm 0:d54bb6a949bf 490 // ...
phillippsm 0:d54bb6a949bf 491 // The values returned are in microseconds and range from 0 to
phillippsm 0:d54bb6a949bf 492 // timeout (as specified in the constructor).
phillippsm 0:d54bb6a949bf 493 void QTRSensorsRC::readPrivate(unsigned int *sensor_values)
phillippsm 0:d54bb6a949bf 494 {
phillippsm 0:d54bb6a949bf 495 unsigned char i;
phillippsm 0:d54bb6a949bf 496
phillippsm 0:d54bb6a949bf 497 if (_pins == 0)
phillippsm 0:d54bb6a949bf 498 return;
phillippsm 0:d54bb6a949bf 499
phillippsm 0:d54bb6a949bf 500
phillippsm 0:d54bb6a949bf 501 for(i = 0; i < _numSensors; i++)
phillippsm 0:d54bb6a949bf 502 {
phillippsm 0:d54bb6a949bf 503 sensor_values[i] = _maxValue;
phillippsm 0:d54bb6a949bf 504
phillippsm 0:d54bb6a949bf 505 _qtrPins[i]->write(HIGH); // make sensor line an output
phillippsm 0:d54bb6a949bf 506 //pinMode(_pins[i], OUTPUT); // drive sensor line high
phillippsm 0:d54bb6a949bf 507 }
phillippsm 0:d54bb6a949bf 508
phillippsm 0:d54bb6a949bf 509 wait_ms(10); // charge lines for 10 us
phillippsm 0:d54bb6a949bf 510
phillippsm 0:d54bb6a949bf 511 for(i = 0; i < _numSensors; i++)
phillippsm 0:d54bb6a949bf 512 {
phillippsm 0:d54bb6a949bf 513 // important: disable internal pull-up!
phillippsm 0:d54bb6a949bf 514 _qtrPins[i]->write(LOW);
phillippsm 0:d54bb6a949bf 515 }
phillippsm 0:d54bb6a949bf 516
phillippsm 0:d54bb6a949bf 517 timer.start();
phillippsm 0:d54bb6a949bf 518 unsigned long startTime = timer.read_ms();
phillippsm 0:d54bb6a949bf 519 while ((timer.read_ms() - startTime) < _maxValue)
phillippsm 0:d54bb6a949bf 520 {
phillippsm 0:d54bb6a949bf 521 unsigned int time = timer.read_ms() - startTime;
phillippsm 0:d54bb6a949bf 522 for (i = 0; i < _numSensors; i++)
phillippsm 0:d54bb6a949bf 523 {
phillippsm 0:d54bb6a949bf 524 if (_qtrPins[i]->read() == LOW && time < sensor_values[i])
phillippsm 0:d54bb6a949bf 525 sensor_values[i] = time;
phillippsm 0:d54bb6a949bf 526 }
phillippsm 0:d54bb6a949bf 527 }
phillippsm 0:d54bb6a949bf 528
phillippsm 0:d54bb6a949bf 529 timer.stop();
phillippsm 0:d54bb6a949bf 530 }
phillippsm 0:d54bb6a949bf 531
phillippsm 0:d54bb6a949bf 532
phillippsm 0:d54bb6a949bf 533
phillippsm 0:d54bb6a949bf 534 // Derived Analog class constructors
phillippsm 0:d54bb6a949bf 535 QTRSensorsAnalog::QTRSensorsAnalog()
phillippsm 0:d54bb6a949bf 536 {
phillippsm 0:d54bb6a949bf 537 calibratedMinimumOn = 0;
phillippsm 0:d54bb6a949bf 538 calibratedMaximumOn = 0;
phillippsm 0:d54bb6a949bf 539 calibratedMinimumOff = 0;
phillippsm 0:d54bb6a949bf 540 calibratedMaximumOff = 0;
phillippsm 0:d54bb6a949bf 541 _pins = 0;
phillippsm 0:d54bb6a949bf 542 }
phillippsm 0:d54bb6a949bf 543
phillippsm 0:d54bb6a949bf 544 QTRSensorsAnalog::QTRSensorsAnalog(PinName* pins,
phillippsm 0:d54bb6a949bf 545 unsigned char numSensors,
phillippsm 0:d54bb6a949bf 546 unsigned char numSamplesPerSensor,
phillippsm 0:d54bb6a949bf 547 PinName emitterPin)
phillippsm 0:d54bb6a949bf 548 {
phillippsm 0:d54bb6a949bf 549 calibratedMinimumOn = 0;
phillippsm 0:d54bb6a949bf 550 calibratedMaximumOn = 0;
phillippsm 0:d54bb6a949bf 551 calibratedMinimumOff = 0;
phillippsm 0:d54bb6a949bf 552 calibratedMaximumOff = 0;
phillippsm 0:d54bb6a949bf 553 _pins = 0;
phillippsm 0:d54bb6a949bf 554
phillippsm 0:d54bb6a949bf 555 // this is analog - so use analog = true as a parameter
phillippsm 0:d54bb6a949bf 556
phillippsm 0:d54bb6a949bf 557 init(pins, numSensors, numSamplesPerSensor, emitterPin);
phillippsm 0:d54bb6a949bf 558 }
phillippsm 0:d54bb6a949bf 559
phillippsm 0:d54bb6a949bf 560
phillippsm 0:d54bb6a949bf 561 // the array 'pins' contains the Arduino analog pin assignment for each
phillippsm 0:d54bb6a949bf 562 // sensor. For example, if pins is {0, 1, 7}, sensor 1 is on
phillippsm 0:d54bb6a949bf 563 // Arduino analog input 0, sensor 2 is on Arduino analog input 1,
phillippsm 0:d54bb6a949bf 564 // and sensor 3 is on Arduino analog input 7.
phillippsm 0:d54bb6a949bf 565
phillippsm 0:d54bb6a949bf 566 // 'numSensors' specifies the length of the 'analogPins' array (i.e. the
phillippsm 0:d54bb6a949bf 567 // number of QTR-A sensors you are using). numSensors must be
phillippsm 0:d54bb6a949bf 568 // no greater than 16.
phillippsm 0:d54bb6a949bf 569
phillippsm 0:d54bb6a949bf 570 // 'numSamplesPerSensor' indicates the number of 10-bit analog samples
phillippsm 0:d54bb6a949bf 571 // to average per channel (i.e. per sensor) for each reading. The total
phillippsm 0:d54bb6a949bf 572 // number of analog-to-digital conversions performed will be equal to
phillippsm 0:d54bb6a949bf 573 // numSensors*numSamplesPerSensor. Note that it takes about 100 us to
phillippsm 0:d54bb6a949bf 574 // perform a single analog-to-digital conversion, so:
phillippsm 0:d54bb6a949bf 575 // if numSamplesPerSensor is 4 and numSensors is 6, it will take
phillippsm 0:d54bb6a949bf 576 // 4 * 6 * 100 us = ~2.5 ms to perform a full readLine().
phillippsm 0:d54bb6a949bf 577 // Increasing this parameter increases noise suppression at the cost of
phillippsm 0:d54bb6a949bf 578 // sample rate. The recommended value is 4.
phillippsm 0:d54bb6a949bf 579
phillippsm 0:d54bb6a949bf 580 // 'emitterPin' is the Arduino pin that controls the IR LEDs on the 8RC
phillippsm 0:d54bb6a949bf 581 // modules. If you are using a 1RC (i.e. if there is no emitter pin),
phillippsm 0:d54bb6a949bf 582 // or if you just want the emitters on all the time and don't want to
phillippsm 0:d54bb6a949bf 583 // use an I/O pin to control it, use a value of 255 (QTR_NO_EMITTER_PIN).
phillippsm 0:d54bb6a949bf 584 void QTRSensorsAnalog::init(PinName* pins,
phillippsm 0:d54bb6a949bf 585 unsigned char numSensors,
phillippsm 0:d54bb6a949bf 586 unsigned char numSamplesPerSensor,
phillippsm 0:d54bb6a949bf 587 PinName emitterPin)
phillippsm 0:d54bb6a949bf 588 {
phillippsm 0:d54bb6a949bf 589 QTRSensors::init(pins, numSensors, emitterPin, true);
phillippsm 0:d54bb6a949bf 590
phillippsm 0:d54bb6a949bf 591 _numSamplesPerSensor = numSamplesPerSensor;
phillippsm 0:d54bb6a949bf 592 _maxValue = 1023; // this is the maximum returned by the A/D conversion
phillippsm 0:d54bb6a949bf 593 }
phillippsm 0:d54bb6a949bf 594
phillippsm 0:d54bb6a949bf 595
phillippsm 0:d54bb6a949bf 596 // Reads the sensor values into an array. There *MUST* be space
phillippsm 0:d54bb6a949bf 597 // for as many values as there were sensors specified in the constructor.
phillippsm 0:d54bb6a949bf 598 // Example usage:
phillippsm 0:d54bb6a949bf 599 // unsigned int sensor_values[8];
phillippsm 0:d54bb6a949bf 600 // sensors.read(sensor_values);
phillippsm 0:d54bb6a949bf 601 // The values returned are a measure of the reflectance in terms of a
phillippsm 0:d54bb6a949bf 602 // 10-bit ADC average with higher values corresponding to lower
phillippsm 0:d54bb6a949bf 603 // reflectance (e.g. a black surface or a void).
phillippsm 0:d54bb6a949bf 604 void QTRSensorsAnalog::readPrivate(unsigned int *sensor_values)
phillippsm 0:d54bb6a949bf 605 {
phillippsm 0:d54bb6a949bf 606 unsigned char i, j;
phillippsm 0:d54bb6a949bf 607
phillippsm 0:d54bb6a949bf 608 if (_pins == 0)
phillippsm 0:d54bb6a949bf 609 return;
phillippsm 0:d54bb6a949bf 610
phillippsm 0:d54bb6a949bf 611 // reset the values
phillippsm 0:d54bb6a949bf 612 for(i = 0; i < _numSensors; i++)
phillippsm 0:d54bb6a949bf 613 sensor_values[i] = 0;
phillippsm 0:d54bb6a949bf 614
phillippsm 0:d54bb6a949bf 615 for (j = 0; j < _numSamplesPerSensor; j++)
phillippsm 0:d54bb6a949bf 616 {
phillippsm 0:d54bb6a949bf 617 for (i = 0; i < _numSensors; i++)
phillippsm 0:d54bb6a949bf 618 {
phillippsm 0:d54bb6a949bf 619 sensor_values[i] += (unsigned int) _qtrAIPins[i]->read_u16(); // add the conversion result
phillippsm 0:d54bb6a949bf 620 }
phillippsm 0:d54bb6a949bf 621 }
phillippsm 0:d54bb6a949bf 622
phillippsm 0:d54bb6a949bf 623 // get the rounded average of the readings for each sensor
phillippsm 0:d54bb6a949bf 624 for (i = 0; i < _numSensors; i++)
phillippsm 0:d54bb6a949bf 625 sensor_values[i] = (sensor_values[i] + (_numSamplesPerSensor >> 1)) /
phillippsm 0:d54bb6a949bf 626 _numSamplesPerSensor;
phillippsm 0:d54bb6a949bf 627 }
phillippsm 0:d54bb6a949bf 628
phillippsm 0:d54bb6a949bf 629 // the destructor frees up allocated memory
phillippsm 0:d54bb6a949bf 630 QTRSensors::~QTRSensors()
phillippsm 0:d54bb6a949bf 631 {
phillippsm 0:d54bb6a949bf 632 if(calibratedMinimumOff)
phillippsm 0:d54bb6a949bf 633 free(calibratedMinimumOff);
phillippsm 0:d54bb6a949bf 634 if(calibratedMinimumOn)
phillippsm 0:d54bb6a949bf 635 free(calibratedMinimumOn);
phillippsm 0:d54bb6a949bf 636 if(calibratedMaximumOff)
phillippsm 0:d54bb6a949bf 637 free(calibratedMaximumOff);
phillippsm 0:d54bb6a949bf 638 if(calibratedMaximumOn)
phillippsm 0:d54bb6a949bf 639 free(calibratedMaximumOn);
phillippsm 0:d54bb6a949bf 640 if (_pins)
phillippsm 0:d54bb6a949bf 641 free(_pins);
phillippsm 0:d54bb6a949bf 642 unsigned int i;
phillippsm 0:d54bb6a949bf 643 for (i = 0; i < _numSensors; i++) {
phillippsm 0:d54bb6a949bf 644 if (_analog) {
phillippsm 0:d54bb6a949bf 645 delete _qtrAIPins[i];
phillippsm 0:d54bb6a949bf 646 } else {
phillippsm 0:d54bb6a949bf 647 delete _qtrPins[i];
phillippsm 0:d54bb6a949bf 648 }
phillippsm 0:d54bb6a949bf 649 }
phillippsm 0:d54bb6a949bf 650 if (_analog) {
phillippsm 0:d54bb6a949bf 651 _qtrAIPins.clear();
phillippsm 0:d54bb6a949bf 652 vector<AnalogIn *>().swap(_qtrAIPins);
phillippsm 0:d54bb6a949bf 653 } else {
phillippsm 0:d54bb6a949bf 654 _qtrPins.clear();
phillippsm 0:d54bb6a949bf 655 vector<DigitalInOut *>().swap(_qtrPins);
phillippsm 0:d54bb6a949bf 656 }
phillippsm 0:d54bb6a949bf 657 }