701 lines
18 KiB
C++
701 lines
18 KiB
C++
#include "QTRSensors.h"
|
|
#include <Arduino.h>
|
|
|
|
void QTRSensors::setTypeRC()
|
|
{
|
|
_type = QTRType::RC;
|
|
_maxValue = _timeout;
|
|
}
|
|
|
|
void QTRSensors::setTypeAnalog()
|
|
{
|
|
_type = QTRType::Analog;
|
|
_maxValue = 1023; // Arduino analogRead() returns a 10-bit value by default
|
|
}
|
|
|
|
void QTRSensors::setSensorPins(const uint8_t * pins, uint8_t sensorCount)
|
|
{
|
|
if (sensorCount > QTRMaxSensors) { sensorCount = QTRMaxSensors; }
|
|
|
|
// (Re)allocate and initialize the array if necessary.
|
|
uint8_t * oldSensorPins = _sensorPins;
|
|
_sensorPins = (uint8_t *)realloc(_sensorPins, sizeof(uint8_t) * sensorCount);
|
|
if (_sensorPins == nullptr)
|
|
{
|
|
// Memory allocation failed; don't continue.
|
|
free(oldSensorPins); // deallocate any memory used by old array
|
|
return;
|
|
}
|
|
|
|
for (uint8_t i = 0; i < sensorCount; i++)
|
|
{
|
|
_sensorPins[i] = pins[i];
|
|
}
|
|
|
|
_sensorCount = sensorCount;
|
|
|
|
// Any previous calibration values are no longer valid, and the calibration
|
|
// arrays might need to be reallocated if the sensor count was changed.
|
|
calibrationOn.initialized = false;
|
|
calibrationOff.initialized = false;
|
|
}
|
|
|
|
void QTRSensors::setTimeout(uint16_t timeout)
|
|
{
|
|
if (timeout > 32767) { timeout = 32767; }
|
|
_timeout = timeout;
|
|
if (_type == QTRType::RC) { _maxValue = timeout; }
|
|
}
|
|
|
|
void QTRSensors::setSamplesPerSensor(uint8_t samples)
|
|
{
|
|
if (samples > 64) { samples = 64; }
|
|
_samplesPerSensor = samples;
|
|
}
|
|
|
|
void QTRSensors::setEmitterPin(uint8_t emitterPin)
|
|
{
|
|
releaseEmitterPins();
|
|
|
|
_oddEmitterPin = emitterPin;
|
|
pinMode(_oddEmitterPin, OUTPUT);
|
|
|
|
_emitterPinCount = 1;
|
|
}
|
|
|
|
void QTRSensors::setEmitterPins(uint8_t oddEmitterPin, uint8_t evenEmitterPin)
|
|
{
|
|
releaseEmitterPins();
|
|
|
|
_oddEmitterPin = oddEmitterPin;
|
|
_evenEmitterPin = evenEmitterPin;
|
|
pinMode(_oddEmitterPin, OUTPUT);
|
|
pinMode(_evenEmitterPin, OUTPUT);
|
|
|
|
_emitterPinCount = 2;
|
|
}
|
|
|
|
void QTRSensors::releaseEmitterPins()
|
|
{
|
|
if (_oddEmitterPin != QTRNoEmitterPin)
|
|
{
|
|
pinMode(_oddEmitterPin, INPUT);
|
|
_oddEmitterPin = QTRNoEmitterPin;
|
|
}
|
|
|
|
if (_evenEmitterPin != QTRNoEmitterPin)
|
|
{
|
|
pinMode(_evenEmitterPin, INPUT);
|
|
_evenEmitterPin = QTRNoEmitterPin;
|
|
}
|
|
|
|
_emitterPinCount = 0;
|
|
}
|
|
|
|
void QTRSensors::setDimmingLevel(uint8_t dimmingLevel)
|
|
{
|
|
if (dimmingLevel > 31) { dimmingLevel = 31; }
|
|
_dimmingLevel = dimmingLevel;
|
|
}
|
|
|
|
// emitters defaults to QTREmitters::All; wait defaults to true
|
|
void QTRSensors::emittersOff(QTREmitters emitters, bool wait)
|
|
{
|
|
bool pinChanged = false;
|
|
|
|
// Use odd emitter pin in these cases:
|
|
// - 1 emitter pin, emitters = all
|
|
// - 2 emitter pins, emitters = all
|
|
// - 2 emitter pins, emitters = odd
|
|
if (emitters == QTREmitters::All ||
|
|
(_emitterPinCount == 2 && emitters == QTREmitters::Odd))
|
|
{
|
|
// Check if pin is defined and only turn off if not already off
|
|
if ((_oddEmitterPin != QTRNoEmitterPin) &&
|
|
(digitalRead(_oddEmitterPin) == HIGH))
|
|
{
|
|
digitalWrite(_oddEmitterPin, LOW);
|
|
pinChanged = true;
|
|
}
|
|
}
|
|
|
|
// Use even emitter pin in these cases:
|
|
// - 2 emitter pins, emitters = all
|
|
// - 2 emitter pins, emitters = even
|
|
if (_emitterPinCount == 2 &&
|
|
(emitters == QTREmitters::All || emitters == QTREmitters::Even))
|
|
{
|
|
// Check if pin is defined and only turn off if not already off
|
|
if ((_evenEmitterPin != QTRNoEmitterPin) &&
|
|
(digitalRead(_evenEmitterPin) == HIGH))
|
|
{
|
|
digitalWrite(_evenEmitterPin, LOW);
|
|
pinChanged = true;
|
|
}
|
|
}
|
|
|
|
if (wait && pinChanged)
|
|
{
|
|
if (_dimmable)
|
|
{
|
|
// driver min is 1 ms
|
|
delayMicroseconds(1200);
|
|
}
|
|
else
|
|
{
|
|
delayMicroseconds(200);
|
|
}
|
|
}
|
|
}
|
|
|
|
void QTRSensors::emittersOn(QTREmitters emitters, bool wait)
|
|
{
|
|
bool pinChanged = false;
|
|
uint16_t emittersOnStart;
|
|
|
|
// Use odd emitter pin in these cases:
|
|
// - 1 emitter pin, emitters = all
|
|
// - 2 emitter pins, emitters = all
|
|
// - 2 emitter pins, emitters = odd
|
|
if (emitters == QTREmitters::All ||
|
|
(_emitterPinCount == 2 && emitters == QTREmitters::Odd))
|
|
{
|
|
// Check if pin is defined, and only turn on non-dimmable sensors if not
|
|
// already on, but always turn dimmable sensors off and back on because
|
|
// we might be changing the dimming level (emittersOnWithPin() should take
|
|
// care of this)
|
|
if ((_oddEmitterPin != QTRNoEmitterPin) &&
|
|
( _dimmable || (digitalRead(_oddEmitterPin) == LOW)))
|
|
{
|
|
emittersOnStart = emittersOnWithPin(_oddEmitterPin);
|
|
pinChanged = true;
|
|
}
|
|
}
|
|
|
|
// Use even emitter pin in these cases:
|
|
// - 2 emitter pins, emitters = all
|
|
// - 2 emitter pins, emitters = even
|
|
if (_emitterPinCount == 2 &&
|
|
(emitters == QTREmitters::All || emitters == QTREmitters::Even))
|
|
{
|
|
// Check if pin is defined, and only turn on non-dimmable sensors if not
|
|
// already on, but always turn dimmable sensors off and back on because
|
|
// we might be changing the dimming level (emittersOnWithPin() should take
|
|
// care of this)
|
|
if ((_evenEmitterPin != QTRNoEmitterPin) &&
|
|
(_dimmable || (digitalRead(_evenEmitterPin) == LOW)))
|
|
{
|
|
emittersOnStart = emittersOnWithPin(_evenEmitterPin);
|
|
pinChanged = true;
|
|
}
|
|
}
|
|
|
|
if (wait && pinChanged)
|
|
{
|
|
if (_dimmable)
|
|
{
|
|
// Make sure it's been at least 300 us since the emitter pin was first set
|
|
// high before returning. (Driver min is 250 us.) Some time might have
|
|
// already passed while we set the dimming level.
|
|
while ((uint16_t)(micros() - emittersOnStart) < 300)
|
|
{
|
|
delayMicroseconds(10);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
delayMicroseconds(200);
|
|
}
|
|
}
|
|
}
|
|
|
|
// assumes pin is valid (not QTRNoEmitterPin)
|
|
// returns time when pin was first set high (used by emittersSelect())
|
|
uint16_t QTRSensors::emittersOnWithPin(uint8_t pin)
|
|
{
|
|
if (_dimmable && (digitalRead(pin) == HIGH))
|
|
{
|
|
// We are turning on dimmable emitters that are already on. To avoid messing
|
|
// up the dimming level, we have to turn the emitters off and back on. This
|
|
// means the turn-off delay will happen even if wait = false was passed to
|
|
// emittersOn(). (Driver min is 1 ms.)
|
|
digitalWrite(pin, LOW);
|
|
delayMicroseconds(1200);
|
|
}
|
|
|
|
digitalWrite(pin, HIGH);
|
|
uint16_t emittersOnStart = micros();
|
|
|
|
if (_dimmable && (_dimmingLevel > 0))
|
|
{
|
|
noInterrupts();
|
|
|
|
for (uint8_t i = 0; i < _dimmingLevel; i++)
|
|
{
|
|
delayMicroseconds(1);
|
|
digitalWrite(pin, LOW);
|
|
delayMicroseconds(1);
|
|
digitalWrite(pin, HIGH);
|
|
}
|
|
|
|
interrupts();
|
|
}
|
|
|
|
return emittersOnStart;
|
|
}
|
|
|
|
void QTRSensors::emittersSelect(QTREmitters emitters)
|
|
{
|
|
QTREmitters offEmitters;
|
|
|
|
switch (emitters)
|
|
{
|
|
case QTREmitters::Odd:
|
|
offEmitters = QTREmitters::Even;
|
|
break;
|
|
|
|
case QTREmitters::Even:
|
|
offEmitters = QTREmitters::Odd;
|
|
break;
|
|
|
|
case QTREmitters::All:
|
|
emittersOn();
|
|
return;
|
|
|
|
case QTREmitters::None:
|
|
emittersOff();
|
|
return;
|
|
|
|
default: // invalid
|
|
return;
|
|
}
|
|
|
|
// Turn off the off-emitters; don't wait before proceeding, but record the time.
|
|
emittersOff(offEmitters, false);
|
|
uint16_t turnOffStart = micros();
|
|
|
|
// Turn on the on-emitters and wait.
|
|
emittersOn(emitters);
|
|
|
|
if (_dimmable)
|
|
{
|
|
// Finish waiting for the off-emitters emitters to turn off: make sure it's been
|
|
// at least 1200 us since the off-emitters was turned off before returning.
|
|
// (Driver min is 1 ms.) Some time has already passed while we waited for
|
|
// the on-emitters to turn on.
|
|
while ((uint16_t)(micros() - turnOffStart) < 1200)
|
|
{
|
|
delayMicroseconds(10);
|
|
}
|
|
}
|
|
}
|
|
|
|
void QTRSensors::resetCalibration()
|
|
{
|
|
for (uint8_t i = 0; i < _sensorCount; i++)
|
|
{
|
|
if (calibrationOn.maximum) { calibrationOn.maximum[i] = 0; }
|
|
if (calibrationOff.maximum) { calibrationOff.maximum[i] = 0; }
|
|
if (calibrationOn.minimum) { calibrationOn.minimum[i] = _maxValue; }
|
|
if (calibrationOff.minimum) { calibrationOff.minimum[i] = _maxValue; }
|
|
}
|
|
}
|
|
|
|
void QTRSensors::calibrate(QTRReadMode mode)
|
|
{
|
|
// manual emitter control is not supported
|
|
if (mode == QTRReadMode::Manual) { return; }
|
|
|
|
if (mode == QTRReadMode::On ||
|
|
mode == QTRReadMode::OnAndOff)
|
|
{
|
|
calibrateOnOrOff(calibrationOn, QTRReadMode::On);
|
|
}
|
|
else if (mode == QTRReadMode::OddEven ||
|
|
mode == QTRReadMode::OddEvenAndOff)
|
|
{
|
|
calibrateOnOrOff(calibrationOn, QTRReadMode::OddEven);
|
|
}
|
|
|
|
if (mode == QTRReadMode::OnAndOff ||
|
|
mode == QTRReadMode::OddEvenAndOff ||
|
|
mode == QTRReadMode::Off)
|
|
{
|
|
calibrateOnOrOff(calibrationOff, QTRReadMode::Off);
|
|
}
|
|
}
|
|
|
|
void QTRSensors::calibrateOnOrOff(CalibrationData & calibration, QTRReadMode mode)
|
|
{
|
|
uint16_t sensorValues[QTRMaxSensors];
|
|
uint16_t maxSensorValues[QTRMaxSensors];
|
|
uint16_t minSensorValues[QTRMaxSensors];
|
|
|
|
// (Re)allocate and initialize the arrays if necessary.
|
|
if (!calibration.initialized)
|
|
{
|
|
uint16_t * oldMaximum = calibration.maximum;
|
|
calibration.maximum = (uint16_t *)realloc(calibration.maximum,
|
|
sizeof(uint16_t) * _sensorCount);
|
|
if (calibration.maximum == nullptr)
|
|
{
|
|
// Memory allocation failed; don't continue.
|
|
free(oldMaximum); // deallocate any memory used by old array
|
|
return;
|
|
}
|
|
|
|
uint16_t * oldMinimum = calibration.minimum;
|
|
calibration.minimum = (uint16_t *)realloc(calibration.minimum,
|
|
sizeof(uint16_t) * _sensorCount);
|
|
if (calibration.minimum == nullptr)
|
|
{
|
|
// Memory allocation failed; don't continue.
|
|
free(oldMinimum); // deallocate any memory used by old array
|
|
return;
|
|
}
|
|
|
|
// Initialize the max and min calibrated values to values that
|
|
// will cause the first reading to update them.
|
|
for (uint8_t i = 0; i < _sensorCount; i++)
|
|
{
|
|
calibration.maximum[i] = 0;
|
|
calibration.minimum[i] = _maxValue;
|
|
}
|
|
|
|
calibration.initialized = true;
|
|
}
|
|
|
|
for (uint8_t j = 0; j < 10; j++)
|
|
{
|
|
read(sensorValues, mode);
|
|
|
|
for (uint8_t i = 0; i < _sensorCount; i++)
|
|
{
|
|
// set the max we found THIS time
|
|
if ((j == 0) || (sensorValues[i] > maxSensorValues[i]))
|
|
{
|
|
maxSensorValues[i] = sensorValues[i];
|
|
}
|
|
|
|
// set the min we found THIS time
|
|
if ((j == 0) || (sensorValues[i] < minSensorValues[i]))
|
|
{
|
|
minSensorValues[i] = sensorValues[i];
|
|
}
|
|
}
|
|
}
|
|
|
|
// record the min and max calibration values
|
|
for (uint8_t i = 0; i < _sensorCount; i++)
|
|
{
|
|
// Update maximum only if the min of 10 readings was still higher than it
|
|
// (we got 10 readings in a row higher than the existing maximum).
|
|
if (minSensorValues[i] > calibration.maximum[i])
|
|
{
|
|
calibration.maximum[i] = minSensorValues[i];
|
|
}
|
|
|
|
// Update minimum only if the max of 10 readings was still lower than it
|
|
// (we got 10 readings in a row lower than the existing minimum).
|
|
if (maxSensorValues[i] < calibration.minimum[i])
|
|
{
|
|
calibration.minimum[i] = maxSensorValues[i];
|
|
}
|
|
}
|
|
}
|
|
|
|
void QTRSensors::read(uint16_t * sensorValues, QTRReadMode mode)
|
|
{
|
|
switch (mode)
|
|
{
|
|
case QTRReadMode::Off:
|
|
emittersOff();
|
|
// fall through
|
|
case QTRReadMode::Manual:
|
|
readPrivate(sensorValues);
|
|
return;
|
|
|
|
case QTRReadMode::On:
|
|
case QTRReadMode::OnAndOff:
|
|
emittersOn();
|
|
readPrivate(sensorValues);
|
|
emittersOff();
|
|
break;
|
|
|
|
case QTRReadMode::OddEven:
|
|
case QTRReadMode::OddEvenAndOff:
|
|
// Turn on odd emitters and read the odd-numbered sensors.
|
|
// (readPrivate takes a 0-based array index, so start = 0 to start with
|
|
// the first sensor)
|
|
emittersSelect(QTREmitters::Odd);
|
|
readPrivate(sensorValues, 0, 2);
|
|
|
|
// Turn on even emitters and read the even-numbered sensors.
|
|
// (readPrivate takes a 0-based array index, so start = 1 to start with
|
|
// the second sensor)
|
|
emittersSelect(QTREmitters::Even);
|
|
readPrivate(sensorValues, 1, 2);
|
|
|
|
emittersOff();
|
|
break;
|
|
|
|
default: // invalid - do nothing
|
|
return;
|
|
}
|
|
|
|
if (mode == QTRReadMode::OnAndOff ||
|
|
mode == QTRReadMode::OddEvenAndOff)
|
|
{
|
|
// Take a second set of readings and return the values (on + max - off).
|
|
|
|
uint16_t offValues[QTRMaxSensors];
|
|
readPrivate(offValues);
|
|
|
|
for (uint8_t i = 0; i < _sensorCount; i++)
|
|
{
|
|
sensorValues[i] += _maxValue - offValues[i];
|
|
if (sensorValues[i] > _maxValue)
|
|
{
|
|
// This usually doesn't happen, because the sensor reading should
|
|
// go up when the emitters are turned off.
|
|
sensorValues[i] = _maxValue;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void QTRSensors::readCalibrated(uint16_t * sensorValues, QTRReadMode mode)
|
|
{
|
|
// manual emitter control is not supported
|
|
if (mode == QTRReadMode::Manual) { return; }
|
|
|
|
// if not calibrated, do nothing
|
|
|
|
if (mode == QTRReadMode::On ||
|
|
mode == QTRReadMode::OnAndOff ||
|
|
mode == QTRReadMode::OddEvenAndOff)
|
|
{
|
|
if (!calibrationOn.initialized)
|
|
{
|
|
return;
|
|
}
|
|
}
|
|
|
|
if (mode == QTRReadMode::Off ||
|
|
mode == QTRReadMode::OnAndOff ||
|
|
mode == QTRReadMode::OddEvenAndOff)
|
|
{
|
|
if (!calibrationOff.initialized)
|
|
{
|
|
return;
|
|
}
|
|
}
|
|
|
|
// read the needed values
|
|
read(sensorValues, mode);
|
|
|
|
for (uint8_t i = 0; i < _sensorCount; i++)
|
|
{
|
|
uint16_t calmin, calmax;
|
|
|
|
// find the correct calibration
|
|
if (mode == QTRReadMode::On ||
|
|
mode == QTRReadMode::OddEven)
|
|
{
|
|
calmax = calibrationOn.maximum[i];
|
|
calmin = calibrationOn.minimum[i];
|
|
}
|
|
else if (mode == QTRReadMode::Off)
|
|
{
|
|
calmax = calibrationOff.maximum[i];
|
|
calmin = calibrationOff.minimum[i];
|
|
}
|
|
else // QTRReadMode::OnAndOff, QTRReadMode::OddEvenAndOff
|
|
{
|
|
if (calibrationOff.minimum[i] < calibrationOn.minimum[i])
|
|
{
|
|
// no meaningful signal
|
|
calmin = _maxValue;
|
|
}
|
|
else
|
|
{
|
|
// this won't go past _maxValue
|
|
calmin = calibrationOn.minimum[i] + _maxValue - calibrationOff.minimum[i];
|
|
}
|
|
|
|
if (calibrationOff.maximum[i] < calibrationOn.maximum[i])
|
|
{
|
|
// no meaningful signal
|
|
calmax = _maxValue;
|
|
}
|
|
else
|
|
{
|
|
// this won't go past _maxValue
|
|
calmax = calibrationOn.maximum[i] + _maxValue - calibrationOff.maximum[i];
|
|
}
|
|
}
|
|
|
|
uint16_t denominator = calmax - calmin;
|
|
int16_t value = 0;
|
|
|
|
if (denominator != 0)
|
|
{
|
|
value = (((int32_t)sensorValues[i]) - calmin) * 1000 / denominator;
|
|
}
|
|
|
|
if (value < 0) { value = 0; }
|
|
else if (value > 1000) { value = 1000; }
|
|
|
|
sensorValues[i] = value;
|
|
}
|
|
}
|
|
|
|
// Reads the first of every [step] sensors, starting with [start] (0-indexed, so
|
|
// start = 0 means start with the first sensor).
|
|
// For example, step = 2, start = 1 means read the *even-numbered* sensors.
|
|
// start defaults to 0, step defaults to 1
|
|
void QTRSensors::readPrivate(uint16_t * sensorValues, uint8_t start, uint8_t step)
|
|
{
|
|
if (_sensorPins == nullptr) { return; }
|
|
|
|
switch (_type)
|
|
{
|
|
case QTRType::RC:
|
|
for (uint8_t i = start; i < _sensorCount; i += step)
|
|
{
|
|
sensorValues[i] = _maxValue;
|
|
// make sensor line an output (drives low briefly, but doesn't matter)
|
|
pinMode(_sensorPins[i], OUTPUT);
|
|
// drive sensor line high
|
|
digitalWrite(_sensorPins[i], HIGH);
|
|
}
|
|
|
|
delayMicroseconds(10); // charge lines for 10 us
|
|
|
|
{
|
|
// disable interrupts so we can switch all the pins as close to the same
|
|
// time as possible
|
|
noInterrupts();
|
|
|
|
// record start time before the first sensor is switched to input
|
|
// (similarly, time is checked before the first sensor is read in the
|
|
// loop below)
|
|
uint32_t startTime = micros();
|
|
uint16_t time = 0;
|
|
|
|
for (uint8_t i = start; i < _sensorCount; i += step)
|
|
{
|
|
// make sensor line an input (should also ensure pull-up is disabled)
|
|
pinMode(_sensorPins[i], INPUT);
|
|
}
|
|
|
|
interrupts(); // re-enable
|
|
|
|
while (time < _maxValue)
|
|
{
|
|
// disable interrupts so we can read all the pins as close to the same
|
|
// time as possible
|
|
noInterrupts();
|
|
|
|
time = micros() - startTime;
|
|
for (uint8_t i = start; i < _sensorCount; i += step)
|
|
{
|
|
if ((digitalRead(_sensorPins[i]) == LOW) && (time < sensorValues[i]))
|
|
{
|
|
// record the first time the line reads low
|
|
sensorValues[i] = time;
|
|
}
|
|
}
|
|
|
|
interrupts(); // re-enable
|
|
}
|
|
}
|
|
return;
|
|
|
|
case QTRType::Analog:
|
|
// reset the values
|
|
for (uint8_t i = start; i < _sensorCount; i += step)
|
|
{
|
|
sensorValues[i] = 0;
|
|
}
|
|
|
|
for (uint8_t j = 0; j < _samplesPerSensor; j++)
|
|
{
|
|
for (uint8_t i = start; i < _sensorCount; i += step)
|
|
{
|
|
// add the conversion result
|
|
sensorValues[i] += analogRead(_sensorPins[i]);
|
|
}
|
|
}
|
|
|
|
// get the rounded average of the readings for each sensor
|
|
for (uint8_t i = start; i < _sensorCount; i += step)
|
|
{
|
|
sensorValues[i] = (sensorValues[i] + (_samplesPerSensor >> 1)) /
|
|
_samplesPerSensor;
|
|
}
|
|
return;
|
|
|
|
default: // QTRType::Undefined or invalid - do nothing
|
|
return;
|
|
}
|
|
}
|
|
|
|
uint16_t QTRSensors::readLinePrivate(uint16_t * sensorValues, QTRReadMode mode,
|
|
bool invertReadings)
|
|
{
|
|
bool onLine = false;
|
|
uint32_t avg = 0; // this is for the weighted total
|
|
uint16_t sum = 0; // this is for the denominator, which is <= 64000
|
|
|
|
// manual emitter control is not supported
|
|
if (mode == QTRReadMode::Manual) { return 0; }
|
|
|
|
readCalibrated(sensorValues, mode);
|
|
|
|
for (uint8_t i = 0; i < _sensorCount; i++)
|
|
{
|
|
uint16_t value = sensorValues[i];
|
|
if (invertReadings) { value = 1000 - value; }
|
|
|
|
// keep track of whether we see the line at all
|
|
if (value > 200) { onLine = true; }
|
|
|
|
// only average in values that are above a noise threshold
|
|
if (value > 50)
|
|
{
|
|
avg += (uint32_t)value * (i * 1000);
|
|
sum += value;
|
|
}
|
|
}
|
|
|
|
if (!onLine)
|
|
{
|
|
// If it last read to the left of center, return 0.
|
|
if (_lastPosition < (_sensorCount - 1) * 1000 / 2)
|
|
{
|
|
return 0;
|
|
}
|
|
// If it last read to the right of center, return the max.
|
|
else
|
|
{
|
|
return (_sensorCount - 1) * 1000;
|
|
}
|
|
}
|
|
|
|
_lastPosition = avg / sum;
|
|
return _lastPosition;
|
|
}
|
|
|
|
// the destructor frees up allocated memory
|
|
QTRSensors::~QTRSensors()
|
|
{
|
|
releaseEmitterPins();
|
|
|
|
if (_sensorPins) { free(_sensorPins); }
|
|
if (calibrationOn.maximum) { free(calibrationOn.maximum); }
|
|
if (calibrationOff.maximum) { free(calibrationOff.maximum); }
|
|
if (calibrationOn.minimum) { free(calibrationOn.minimum); }
|
|
if (calibrationOff.minimum) { free(calibrationOff.minimum); }
|
|
}
|