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 | }
|
---|