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