Balboa32U4 library
QTRSensors.cpp
1 #include "QTRSensors.h"
2 #include <Arduino.h>
3 
5 {
6  _type = QTRType::RC;
7  _maxValue = _timeout;
8 }
9 
11 {
12  _type = QTRType::Analog;
13  _maxValue = 1023; // Arduino analogRead() returns a 10-bit value by default
14 }
15 
16 void QTRSensors::setSensorPins(const uint8_t * pins, uint8_t sensorCount)
17 {
18  if (sensorCount > QTRMaxSensors) { sensorCount = QTRMaxSensors; }
19 
20  // (Re)allocate and initialize the array if necessary.
21  uint8_t * oldSensorPins = _sensorPins;
22  _sensorPins = (uint8_t *)realloc(_sensorPins, sizeof(uint8_t) * sensorCount);
23  if (_sensorPins == nullptr)
24  {
25  // Memory allocation failed; don't continue.
26  free(oldSensorPins); // deallocate any memory used by old array
27  return;
28  }
29 
30  for (uint8_t i = 0; i < sensorCount; i++)
31  {
32  _sensorPins[i] = pins[i];
33  }
34 
35  _sensorCount = sensorCount;
36 
37  // Any previous calibration values are no longer valid, and the calibration
38  // arrays might need to be reallocated if the sensor count was changed.
39  calibrationOn.initialized = false;
41 }
42 
43 void QTRSensors::setTimeout(uint16_t timeout)
44 {
45  if (timeout > 32767) { timeout = 32767; }
46  _timeout = timeout;
47  if (_type == QTRType::RC) { _maxValue = timeout; }
48 }
49 
50 void QTRSensors::setSamplesPerSensor(uint8_t samples)
51 {
52  if (samples > 64) { samples = 64; }
53  _samplesPerSensor = samples;
54 }
55 
56 void QTRSensors::setEmitterPin(uint8_t emitterPin)
57 {
59 
60  _oddEmitterPin = emitterPin;
61  pinMode(_oddEmitterPin, OUTPUT);
62 
63  _emitterPinCount = 1;
64 }
65 
66 void QTRSensors::setEmitterPins(uint8_t oddEmitterPin, uint8_t evenEmitterPin)
67 {
69 
70  _oddEmitterPin = oddEmitterPin;
71  _evenEmitterPin = evenEmitterPin;
72  pinMode(_oddEmitterPin, OUTPUT);
73  pinMode(_evenEmitterPin, OUTPUT);
74 
75  _emitterPinCount = 2;
76 }
77 
79 {
80  if (_oddEmitterPin != QTRNoEmitterPin)
81  {
82  pinMode(_oddEmitterPin, INPUT);
83  _oddEmitterPin = QTRNoEmitterPin;
84  }
85 
86  if (_evenEmitterPin != QTRNoEmitterPin)
87  {
88  pinMode(_evenEmitterPin, INPUT);
89  _evenEmitterPin = QTRNoEmitterPin;
90  }
91 
92  _emitterPinCount = 0;
93 }
94 
95 void QTRSensors::setDimmingLevel(uint8_t dimmingLevel)
96 {
97  if (dimmingLevel > 31) { dimmingLevel = 31; }
98  _dimmingLevel = dimmingLevel;
99 }
100 
101 // emitters defaults to QTREmitters::All; wait defaults to true
102 void QTRSensors::emittersOff(QTREmitters emitters, bool wait)
103 {
104  bool pinChanged = false;
105 
106  // Use odd emitter pin in these cases:
107  // - 1 emitter pin, emitters = all
108  // - 2 emitter pins, emitters = all
109  // - 2 emitter pins, emitters = odd
110  if (emitters == QTREmitters::All ||
111  (_emitterPinCount == 2 && emitters == QTREmitters::Odd))
112  {
113  // Check if pin is defined and only turn off if not already off
114  if ((_oddEmitterPin != QTRNoEmitterPin) &&
115  (digitalRead(_oddEmitterPin) == HIGH))
116  {
117  digitalWrite(_oddEmitterPin, LOW);
118  pinChanged = true;
119  }
120  }
121 
122  // Use even emitter pin in these cases:
123  // - 2 emitter pins, emitters = all
124  // - 2 emitter pins, emitters = even
125  if (_emitterPinCount == 2 &&
126  (emitters == QTREmitters::All || emitters == QTREmitters::Even))
127  {
128  // Check if pin is defined and only turn off if not already off
129  if ((_evenEmitterPin != QTRNoEmitterPin) &&
130  (digitalRead(_evenEmitterPin) == HIGH))
131  {
132  digitalWrite(_evenEmitterPin, LOW);
133  pinChanged = true;
134  }
135  }
136 
137  if (wait && pinChanged)
138  {
139  if (_dimmable)
140  {
141  // driver min is 1 ms
142  delayMicroseconds(1200);
143  }
144  else
145  {
146  delayMicroseconds(200);
147  }
148  }
149 }
150 
151 void QTRSensors::emittersOn(QTREmitters emitters, bool wait)
152 {
153  bool pinChanged = false;
154  uint16_t emittersOnStart;
155 
156  // Use odd emitter pin in these cases:
157  // - 1 emitter pin, emitters = all
158  // - 2 emitter pins, emitters = all
159  // - 2 emitter pins, emitters = odd
160  if (emitters == QTREmitters::All ||
161  (_emitterPinCount == 2 && emitters == QTREmitters::Odd))
162  {
163  // Check if pin is defined, and only turn on non-dimmable sensors if not
164  // already on, but always turn dimmable sensors off and back on because
165  // we might be changing the dimming level (emittersOnWithPin() should take
166  // care of this)
167  if ((_oddEmitterPin != QTRNoEmitterPin) &&
168  ( _dimmable || (digitalRead(_oddEmitterPin) == LOW)))
169  {
170  emittersOnStart = emittersOnWithPin(_oddEmitterPin);
171  pinChanged = true;
172  }
173  }
174 
175  // Use even emitter pin in these cases:
176  // - 2 emitter pins, emitters = all
177  // - 2 emitter pins, emitters = even
178  if (_emitterPinCount == 2 &&
179  (emitters == QTREmitters::All || emitters == QTREmitters::Even))
180  {
181  // Check if pin is defined, and only turn on non-dimmable sensors if not
182  // already on, but always turn dimmable sensors off and back on because
183  // we might be changing the dimming level (emittersOnWithPin() should take
184  // care of this)
185  if ((_evenEmitterPin != QTRNoEmitterPin) &&
186  (_dimmable || (digitalRead(_evenEmitterPin) == LOW)))
187  {
188  emittersOnStart = emittersOnWithPin(_evenEmitterPin);
189  pinChanged = true;
190  }
191  }
192 
193  if (wait && pinChanged)
194  {
195  if (_dimmable)
196  {
197  // Make sure it's been at least 300 us since the emitter pin was first set
198  // high before returning. (Driver min is 250 us.) Some time might have
199  // already passed while we set the dimming level.
200  while ((uint16_t)(micros() - emittersOnStart) < 300)
201  {
202  delayMicroseconds(10);
203  }
204  }
205  else
206  {
207  delayMicroseconds(200);
208  }
209  }
210 }
211 
212 // assumes pin is valid (not QTRNoEmitterPin)
213 // returns time when pin was first set high (used by emittersSelect())
214 uint16_t QTRSensors::emittersOnWithPin(uint8_t pin)
215 {
216  if (_dimmable && (digitalRead(pin) == HIGH))
217  {
218  // We are turning on dimmable emitters that are already on. To avoid messing
219  // up the dimming level, we have to turn the emitters off and back on. This
220  // means the turn-off delay will happen even if wait = false was passed to
221  // emittersOn(). (Driver min is 1 ms.)
222  digitalWrite(pin, LOW);
223  delayMicroseconds(1200);
224  }
225 
226  digitalWrite(pin, HIGH);
227  uint16_t emittersOnStart = micros();
228 
229  if (_dimmable && (_dimmingLevel > 0))
230  {
231  noInterrupts();
232 
233  for (uint8_t i = 0; i < _dimmingLevel; i++)
234  {
235  delayMicroseconds(1);
236  digitalWrite(pin, LOW);
237  delayMicroseconds(1);
238  digitalWrite(pin, HIGH);
239  }
240 
241  interrupts();
242  }
243 
244  return emittersOnStart;
245 }
246 
248 {
249  QTREmitters offEmitters;
250 
251  switch (emitters)
252  {
253  case QTREmitters::Odd:
254  offEmitters = QTREmitters::Even;
255  break;
256 
257  case QTREmitters::Even:
258  offEmitters = QTREmitters::Odd;
259  break;
260 
261  case QTREmitters::All:
262  emittersOn();
263  return;
264 
265  case QTREmitters::None:
266  emittersOff();
267  return;
268 
269  default: // invalid
270  return;
271  }
272 
273  // Turn off the off-emitters; don't wait before proceeding, but record the time.
274  emittersOff(offEmitters, false);
275  uint16_t turnOffStart = micros();
276 
277  // Turn on the on-emitters and wait.
278  emittersOn(emitters);
279 
280  if (_dimmable)
281  {
282  // Finish waiting for the off-emitters emitters to turn off: make sure it's been
283  // at least 1200 us since the off-emitters was turned off before returning.
284  // (Driver min is 1 ms.) Some time has already passed while we waited for
285  // the on-emitters to turn on.
286  while ((uint16_t)(micros() - turnOffStart) < 1200)
287  {
288  delayMicroseconds(10);
289  }
290  }
291 }
292 
294 {
295  for (uint8_t i = 0; i < _sensorCount; i++)
296  {
297  if (calibrationOn.maximum) { calibrationOn.maximum[i] = 0; }
299  if (calibrationOn.minimum) { calibrationOn.minimum[i] = _maxValue; }
300  if (calibrationOff.minimum) { calibrationOff.minimum[i] = _maxValue; }
301  }
302 }
303 
305 {
306  // manual emitter control is not supported
307  if (mode == QTRReadMode::Manual) { return; }
308 
309  if (mode == QTRReadMode::On ||
310  mode == QTRReadMode::OnAndOff)
311  {
312  calibrateOnOrOff(calibrationOn, QTRReadMode::On);
313  }
314  else if (mode == QTRReadMode::OddEven ||
316  {
317  calibrateOnOrOff(calibrationOn, QTRReadMode::OddEven);
318  }
319 
320  if (mode == QTRReadMode::OnAndOff ||
321  mode == QTRReadMode::OddEvenAndOff ||
322  mode == QTRReadMode::Off)
323  {
324  calibrateOnOrOff(calibrationOff, QTRReadMode::Off);
325  }
326 }
327 
328 void QTRSensors::calibrateOnOrOff(CalibrationData & calibration, QTRReadMode mode)
329 {
330  uint16_t sensorValues[QTRMaxSensors];
331  uint16_t maxSensorValues[QTRMaxSensors];
332  uint16_t minSensorValues[QTRMaxSensors];
333 
334  // (Re)allocate and initialize the arrays if necessary.
335  if (!calibration.initialized)
336  {
337  uint16_t * oldMaximum = calibration.maximum;
338  calibration.maximum = (uint16_t *)realloc(calibration.maximum,
339  sizeof(uint16_t) * _sensorCount);
340  if (calibration.maximum == nullptr)
341  {
342  // Memory allocation failed; don't continue.
343  free(oldMaximum); // deallocate any memory used by old array
344  return;
345  }
346 
347  uint16_t * oldMinimum = calibration.minimum;
348  calibration.minimum = (uint16_t *)realloc(calibration.minimum,
349  sizeof(uint16_t) * _sensorCount);
350  if (calibration.minimum == nullptr)
351  {
352  // Memory allocation failed; don't continue.
353  free(oldMinimum); // deallocate any memory used by old array
354  return;
355  }
356 
357  // Initialize the max and min calibrated values to values that
358  // will cause the first reading to update them.
359  for (uint8_t i = 0; i < _sensorCount; i++)
360  {
361  calibration.maximum[i] = 0;
362  calibration.minimum[i] = _maxValue;
363  }
364 
365  calibration.initialized = true;
366  }
367 
368  for (uint8_t j = 0; j < 10; j++)
369  {
370  read(sensorValues, mode);
371 
372  for (uint8_t i = 0; i < _sensorCount; i++)
373  {
374  // set the max we found THIS time
375  if ((j == 0) || (sensorValues[i] > maxSensorValues[i]))
376  {
377  maxSensorValues[i] = sensorValues[i];
378  }
379 
380  // set the min we found THIS time
381  if ((j == 0) || (sensorValues[i] < minSensorValues[i]))
382  {
383  minSensorValues[i] = sensorValues[i];
384  }
385  }
386  }
387 
388  // record the min and max calibration values
389  for (uint8_t i = 0; i < _sensorCount; i++)
390  {
391  // Update maximum only if the min of 10 readings was still higher than it
392  // (we got 10 readings in a row higher than the existing maximum).
393  if (minSensorValues[i] > calibration.maximum[i])
394  {
395  calibration.maximum[i] = minSensorValues[i];
396  }
397 
398  // Update minimum only if the max of 10 readings was still lower than it
399  // (we got 10 readings in a row lower than the existing minimum).
400  if (maxSensorValues[i] < calibration.minimum[i])
401  {
402  calibration.minimum[i] = maxSensorValues[i];
403  }
404  }
405 }
406 
407 void QTRSensors::read(uint16_t * sensorValues, QTRReadMode mode)
408 {
409  switch (mode)
410  {
411  case QTRReadMode::Off:
412  emittersOff();
413  // fall through
414  case QTRReadMode::Manual:
415  readPrivate(sensorValues);
416  return;
417 
418  case QTRReadMode::On:
420  emittersOn();
421  readPrivate(sensorValues);
422  emittersOff();
423  break;
424 
427  // Turn on odd emitters and read the odd-numbered sensors.
428  // (readPrivate takes a 0-based array index, so start = 0 to start with
429  // the first sensor)
430  emittersSelect(QTREmitters::Odd);
431  readPrivate(sensorValues, 0, 2);
432 
433  // Turn on even emitters and read the even-numbered sensors.
434  // (readPrivate takes a 0-based array index, so start = 1 to start with
435  // the second sensor)
436  emittersSelect(QTREmitters::Even);
437  readPrivate(sensorValues, 1, 2);
438 
439  emittersOff();
440  break;
441 
442  default: // invalid - do nothing
443  return;
444  }
445 
446  if (mode == QTRReadMode::OnAndOff ||
448  {
449  // Take a second set of readings and return the values (on + max - off).
450 
451  uint16_t offValues[QTRMaxSensors];
452  readPrivate(offValues);
453 
454  for (uint8_t i = 0; i < _sensorCount; i++)
455  {
456  sensorValues[i] += _maxValue - offValues[i];
457  if (sensorValues[i] > _maxValue)
458  {
459  // This usually doesn't happen, because the sensor reading should
460  // go up when the emitters are turned off.
461  sensorValues[i] = _maxValue;
462  }
463  }
464  }
465 }
466 
467 void QTRSensors::readCalibrated(uint16_t * sensorValues, QTRReadMode mode)
468 {
469  // manual emitter control is not supported
470  if (mode == QTRReadMode::Manual) { return; }
471 
472  // if not calibrated, do nothing
473 
474  if (mode == QTRReadMode::On ||
475  mode == QTRReadMode::OnAndOff ||
477  {
479  {
480  return;
481  }
482  }
483 
484  if (mode == QTRReadMode::Off ||
485  mode == QTRReadMode::OnAndOff ||
487  {
489  {
490  return;
491  }
492  }
493 
494  // read the needed values
495  read(sensorValues, mode);
496 
497  for (uint8_t i = 0; i < _sensorCount; i++)
498  {
499  uint16_t calmin, calmax;
500 
501  // find the correct calibration
502  if (mode == QTRReadMode::On ||
503  mode == QTRReadMode::OddEven)
504  {
505  calmax = calibrationOn.maximum[i];
506  calmin = calibrationOn.minimum[i];
507  }
508  else if (mode == QTRReadMode::Off)
509  {
510  calmax = calibrationOff.maximum[i];
511  calmin = calibrationOff.minimum[i];
512  }
513  else // QTRReadMode::OnAndOff, QTRReadMode::OddEvenAndOff
514  {
516  {
517  // no meaningful signal
518  calmin = _maxValue;
519  }
520  else
521  {
522  // this won't go past _maxValue
523  calmin = calibrationOn.minimum[i] + _maxValue - calibrationOff.minimum[i];
524  }
525 
527  {
528  // no meaningful signal
529  calmax = _maxValue;
530  }
531  else
532  {
533  // this won't go past _maxValue
534  calmax = calibrationOn.maximum[i] + _maxValue - calibrationOff.maximum[i];
535  }
536  }
537 
538  uint16_t denominator = calmax - calmin;
539  int16_t value = 0;
540 
541  if (denominator != 0)
542  {
543  value = (((int32_t)sensorValues[i]) - calmin) * 1000 / denominator;
544  }
545 
546  if (value < 0) { value = 0; }
547  else if (value > 1000) { value = 1000; }
548 
549  sensorValues[i] = value;
550  }
551 }
552 
553 // Reads the first of every [step] sensors, starting with [start] (0-indexed, so
554 // start = 0 means start with the first sensor).
555 // For example, step = 2, start = 1 means read the *even-numbered* sensors.
556 // start defaults to 0, step defaults to 1
557 void QTRSensors::readPrivate(uint16_t * sensorValues, uint8_t start, uint8_t step)
558 {
559  if (_sensorPins == nullptr) { return; }
560 
561  switch (_type)
562  {
563  case QTRType::RC:
564  for (uint8_t i = start; i < _sensorCount; i += step)
565  {
566  sensorValues[i] = _maxValue;
567  // make sensor line an output (drives low briefly, but doesn't matter)
568  pinMode(_sensorPins[i], OUTPUT);
569  // drive sensor line high
570  digitalWrite(_sensorPins[i], HIGH);
571  }
572 
573  delayMicroseconds(10); // charge lines for 10 us
574 
575  {
576  // disable interrupts so we can switch all the pins as close to the same
577  // time as possible
578  noInterrupts();
579 
580  // record start time before the first sensor is switched to input
581  // (similarly, time is checked before the first sensor is read in the
582  // loop below)
583  uint32_t startTime = micros();
584  uint16_t time = 0;
585 
586  for (uint8_t i = start; i < _sensorCount; i += step)
587  {
588  // make sensor line an input (should also ensure pull-up is disabled)
589  pinMode(_sensorPins[i], INPUT);
590  }
591 
592  interrupts(); // re-enable
593 
594  while (time < _maxValue)
595  {
596  // disable interrupts so we can read all the pins as close to the same
597  // time as possible
598  noInterrupts();
599 
600  time = micros() - startTime;
601  for (uint8_t i = start; i < _sensorCount; i += step)
602  {
603  if ((digitalRead(_sensorPins[i]) == LOW) && (time < sensorValues[i]))
604  {
605  // record the first time the line reads low
606  sensorValues[i] = time;
607  }
608  }
609 
610  interrupts(); // re-enable
611  }
612  }
613  return;
614 
615  case QTRType::Analog:
616  // reset the values
617  for (uint8_t i = start; i < _sensorCount; i += step)
618  {
619  sensorValues[i] = 0;
620  }
621 
622  for (uint8_t j = 0; j < _samplesPerSensor; j++)
623  {
624  for (uint8_t i = start; i < _sensorCount; i += step)
625  {
626  // add the conversion result
627  sensorValues[i] += analogRead(_sensorPins[i]);
628  }
629  }
630 
631  // get the rounded average of the readings for each sensor
632  for (uint8_t i = start; i < _sensorCount; i += step)
633  {
634  sensorValues[i] = (sensorValues[i] + (_samplesPerSensor >> 1)) /
635  _samplesPerSensor;
636  }
637  return;
638 
639  default: // QTRType::Undefined or invalid - do nothing
640  return;
641  }
642 }
643 
644 uint16_t QTRSensors::readLinePrivate(uint16_t * sensorValues, QTRReadMode mode,
645  bool invertReadings)
646 {
647  bool onLine = false;
648  uint32_t avg = 0; // this is for the weighted total
649  uint16_t sum = 0; // this is for the denominator, which is <= 64000
650 
651  // manual emitter control is not supported
652  if (mode == QTRReadMode::Manual) { return 0; }
653 
654  readCalibrated(sensorValues, mode);
655 
656  for (uint8_t i = 0; i < _sensorCount; i++)
657  {
658  uint16_t value = sensorValues[i];
659  if (invertReadings) { value = 1000 - value; }
660 
661  // keep track of whether we see the line at all
662  if (value > 200) { onLine = true; }
663 
664  // only average in values that are above a noise threshold
665  if (value > 50)
666  {
667  avg += (uint32_t)value * (i * 1000);
668  sum += value;
669  }
670  }
671 
672  if (!onLine)
673  {
674  // If it last read to the left of center, return 0.
675  if (_lastPosition < (_sensorCount - 1) * 1000 / 2)
676  {
677  return 0;
678  }
679  // If it last read to the right of center, return the max.
680  else
681  {
682  return (_sensorCount - 1) * 1000;
683  }
684  }
685 
686  _lastPosition = avg / sum;
687  return _lastPosition;
688 }
689 
690 // the destructor frees up allocated memory
691 QTRSensors::~QTRSensors()
692 {
694 
695  if (_sensorPins) { free(_sensorPins); }
700 }
const uint8_t QTRMaxSensors
The maximum number of sensors supported by an instance of this class.
Definition: QTRSensors.h:71
QTREmitters
Emitters selected to turn on or off.
Definition: QTRSensors.h:57
void calibrate(QTRReadMode mode=QTRReadMode::On)
Reads the sensors for calibration.
Definition: QTRSensors.cpp:304
const uint8_t QTRNoEmitterPin
Represents an undefined emitter control pin.
Definition: QTRSensors.h:65
void setSamplesPerSensor(uint8_t samples)
Sets the number of analog readings to average per analog sensor.
Definition: QTRSensors.cpp:50
void setEmitterPins(uint8_t oddEmitterPin, uint8_t evenEmitterPin)
Sets separate odd and even emitter control pins for the sensors.
Definition: QTRSensors.cpp:66
void setTypeRC()
Specifies that the sensors are RC.
Definition: QTRSensors.cpp:4
void setTimeout(uint16_t timeout)
Sets the timeout for RC sensors.
Definition: QTRSensors.cpp:43
void setEmitterPin(uint8_t emitterPin)
Sets the emitter control pin for the sensors.
Definition: QTRSensors.cpp:56
void setSensorPins(const uint8_t *pins, uint8_t sensorCount)
Sets the sensor pins.
Definition: QTRSensors.cpp:16
QTRReadMode
Emitter behavior when taking readings.
Definition: QTRSensors.h:13
void readCalibrated(uint16_t *sensorValues, QTRReadMode mode=QTRReadMode::On)
Reads the sensors and provides calibrated values between 0 and 1000.
Definition: QTRSensors.cpp:467
uint16_t * minimum
Lowest readings seen during calibration.
Definition: QTRSensors.h:540
void resetCalibration()
Resets all calibration that has been done.
Definition: QTRSensors.cpp:293
void read(uint16_t *sensorValues, QTRReadMode mode=QTRReadMode::On)
Reads the raw sensor values into an array.
Definition: QTRSensors.cpp:407
CalibrationData calibrationOff
Data from calibrating with emitters off.
Definition: QTRSensors.h:558
uint16_t * maximum
Highest readings seen during calibration.
Definition: QTRSensors.h:542
void emittersOff(QTREmitters emitters=QTREmitters::All, bool wait=true)
Turns the IR LEDs off.
Definition: QTRSensors.cpp:102
CalibrationData calibrationOn
Data from calibrating with emitters on.
Definition: QTRSensors.h:555
void emittersSelect(QTREmitters emitters)
Turns on the selected emitters and turns off the other emitters with optimized timing.
Definition: QTRSensors.cpp:247
void setDimmingLevel(uint8_t dimmingLevel)
Sets the dimming level.
Definition: QTRSensors.cpp:95
void emittersOn(QTREmitters emitters=QTREmitters::All, bool wait=true)
Turns the IR LEDs on.
Definition: QTRSensors.cpp:151
void setTypeAnalog()
Specifies that the sensor type is analog.
Definition: QTRSensors.cpp:10
void releaseEmitterPins()
Releases emitter pin/pins that have been set.
Definition: QTRSensors.cpp:78
bool initialized
Whether array pointers have been allocated and initialized.
Definition: QTRSensors.h:538