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