David's line following code from the LVBots competition, 2015.
Dependencies: GeneralDebouncer Pacer PololuEncoder mbed
Fork of DeadReckoning by
Diff: line_tracker.cpp
- Revision:
- 21:c279c6a83671
- Child:
- 22:44c032e59ff5
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/line_tracker.cpp Thu Feb 27 23:20:34 2014 +0000 @@ -0,0 +1,133 @@ +#include "line_tracker.h" + +static uint16_t readSensor(uint8_t index) +{ + return lineSensorsAnalog[index].read_u16(); +} + +LineTracker::LineTracker() +{ + for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++) + { + calibratedMaximum[s] = 0; + calibratedMinimum[s] = 0xFFFF; + } +} + +void LineTracker::updateCalibration() +{ + const int sampleCount = 10; + + uint16_t maxValues[LINE_SENSOR_COUNT]; + uint16_t minValues[LINE_SENSOR_COUNT]; + + for(uint8_t sample = 0; sample < sampleCount; sample++) + { + for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++) + { + uint16_t reading = readSensor(s); + if (reading > maxValues[s]) { maxValues[s] = reading; } + if (reading < minValues[s]) { minValues[s] = reading; } + } + } + + for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++) + { + if (minValues[s] > calibratedMaximum[s]) { calibratedMaximum[s] = minValues[s]; } + if (maxValues[s] > calibratedMinimum[s]) { calibratedMinimum[s] = maxValues[s]; } + } +} + +void LineTracker::read() +{ + readRawValues(); + updateCalibratedValues(); + updateLineStatus(); +} + +void LineTracker::readRawValues() +{ + for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++) + { + rawValues[s] = readSensor(s); + } +} + +void LineTracker::updateCalibratedValues() +{ + // Update the calibrated values. + for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++) + { + uint16_t calmin = calibratedMinimum[s]; + uint16_t calmax = calibratedMaximum[s]; + uint16_t denominator = calmax - calmin; + int16_t x = 0; + if(denominator != 0) + { + x = ((int32_t)rawValues[s] - calmin) * 1000 / denominator; + if(x < 0) + { + x = 0; + } + else if(x > 1000) + { + x = 1000; + } + } + calibratedValues[s] = x; + } +} + +void LineTracker::updateLineStatus() +{ + uint32_t avg = 0; + uint32_t sum = 0; + + for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++) + { + // keep track of whether we see the line at all + uint16_t value = calibratedValues[s]; + if (value > 200) + { + lineVisible = true; + } + + // only average in values that are above a noise threshold + if (value > 50) + { + avg += (uint32_t)(value) * s * 1000; + sum += value; + } + } + + if (lineVisible) + { + linePosition = avg/sum; + } + else + { + // We cannot see the line, so just snap the position to the left-most or right-most + // depending on what we saw previousl. + + const uint32_t max = (LINE_SENSOR_COUNT-1)*1000; + if(linePosition < max/2) + { + linePosition = 0; + } + else + { + linePosition = max; + } + } +} + +// The return value of this should only be heeded if the calibration seems to be OK. +bool LineTracker::getLineVisible() +{ + return lineVisible; +} + +uint16_t LineTracker::getLinePosition() +{ + return linePosition; +} \ No newline at end of file