David's dead reckoning code for the LVBots competition on March 6th. Uses the mbed LPC1768, DRV8835, QTR-3RC, and two DC motors with encoders.
Dependencies: PololuEncoder Pacer mbed GeneralDebouncer
line_tracker.cpp
- Committer:
- DavidEGrayson
- Date:
- 2014-03-04
- Revision:
- 32:83a13b06093c
- Parent:
- 31:739b91331f31
- Child:
- 46:df2c2d25c070
File content as of revision 32:83a13b06093c:
#include "line_tracker.h" LineTracker::LineTracker() { for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++) { calibratedMaximum[s] = 0; calibratedMinimum[s] = 0xFFFF; } calibrationState = 0; } void LineTracker::read() { readRawValues(); updateCalibratedValues(); updateLineStatus(); } void LineTracker::readRawValues() { readSensors(rawValues); } void LineTracker::updateCalibratedValues() { 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; int32_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; lineVisible = false; 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 > 500) { 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; } void LineTracker::updateCalibration() { if(calibrationState == 0) { for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++) { recentValuesMin[s] = 0xFFFF; recentValuesMax[s] = 0; } } for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++) { uint16_t value = rawValues[s]; if (value < recentValuesMin[s]) { recentValuesMin[s] = value; } if (value > recentValuesMax[s]) { recentValuesMax[s] = value; } } calibrationState = calibrationState + 1; if (calibrationState == 9) { calibrationState = 0; for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++) { if (recentValuesMin[s] > calibratedMaximum[s]) { calibratedMaximum[s] = recentValuesMin[s]; } if (recentValuesMax[s] < calibratedMinimum[s]) { calibratedMinimum[s] = recentValuesMax[s]; } } } }