David Grayson / Mbed 2 deprecated DeadReckoning

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@24:fc01d9125d3b, 2014-02-28 (annotated)

Committer:
DavidEGrayson
Date:
Fri Feb 28 01:07:14 2014 +0000
Revision:
24:fc01d9125d3b
Parent:
22:44c032e59ff5
Child:
31:739b91331f31
Fixed the problem with LineTracker always thinking the line was visible.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DavidEGrayson 21:c279c6a83671 1 #include "line_tracker.h"
DavidEGrayson 21:c279c6a83671 2
DavidEGrayson 21:c279c6a83671 3 static uint16_t readSensor(uint8_t index)
DavidEGrayson 21:c279c6a83671 4 {
DavidEGrayson 21:c279c6a83671 5 return lineSensorsAnalog[index].read_u16();
DavidEGrayson 21:c279c6a83671 6 }
DavidEGrayson 21:c279c6a83671 7
DavidEGrayson 21:c279c6a83671 8 LineTracker::LineTracker()
DavidEGrayson 21:c279c6a83671 9 {
DavidEGrayson 21:c279c6a83671 10 for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
DavidEGrayson 21:c279c6a83671 11 {
DavidEGrayson 21:c279c6a83671 12 calibratedMaximum[s] = 0;
DavidEGrayson 21:c279c6a83671 13 calibratedMinimum[s] = 0xFFFF;
DavidEGrayson 21:c279c6a83671 14 }
DavidEGrayson 22:44c032e59ff5 15 calibrationState = 0;
DavidEGrayson 21:c279c6a83671 16 }
DavidEGrayson 21:c279c6a83671 17
DavidEGrayson 21:c279c6a83671 18 void LineTracker::read()
DavidEGrayson 21:c279c6a83671 19 {
DavidEGrayson 21:c279c6a83671 20 readRawValues();
DavidEGrayson 21:c279c6a83671 21 updateCalibratedValues();
DavidEGrayson 21:c279c6a83671 22 updateLineStatus();
DavidEGrayson 21:c279c6a83671 23 }
DavidEGrayson 21:c279c6a83671 24
DavidEGrayson 21:c279c6a83671 25 void LineTracker::readRawValues()
DavidEGrayson 21:c279c6a83671 26 {
DavidEGrayson 21:c279c6a83671 27 for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
DavidEGrayson 21:c279c6a83671 28 {
DavidEGrayson 21:c279c6a83671 29 rawValues[s] = readSensor(s);
DavidEGrayson 21:c279c6a83671 30 }
DavidEGrayson 21:c279c6a83671 31 }
DavidEGrayson 21:c279c6a83671 32
DavidEGrayson 21:c279c6a83671 33 void LineTracker::updateCalibratedValues()
DavidEGrayson 21:c279c6a83671 34 {
DavidEGrayson 21:c279c6a83671 35 for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
DavidEGrayson 21:c279c6a83671 36 {
DavidEGrayson 21:c279c6a83671 37 uint16_t calmin = calibratedMinimum[s];
DavidEGrayson 21:c279c6a83671 38 uint16_t calmax = calibratedMaximum[s];
DavidEGrayson 21:c279c6a83671 39 uint16_t denominator = calmax - calmin;
DavidEGrayson 22:44c032e59ff5 40 int32_t x = 0;
DavidEGrayson 21:c279c6a83671 41 if(denominator != 0)
DavidEGrayson 21:c279c6a83671 42 {
DavidEGrayson 21:c279c6a83671 43 x = ((int32_t)rawValues[s] - calmin) * 1000 / denominator;
DavidEGrayson 21:c279c6a83671 44 if(x < 0)
DavidEGrayson 21:c279c6a83671 45 {
DavidEGrayson 21:c279c6a83671 46 x = 0;
DavidEGrayson 21:c279c6a83671 47 }
DavidEGrayson 21:c279c6a83671 48 else if(x > 1000)
DavidEGrayson 21:c279c6a83671 49 {
DavidEGrayson 21:c279c6a83671 50 x = 1000;
DavidEGrayson 21:c279c6a83671 51 }
DavidEGrayson 21:c279c6a83671 52 }
DavidEGrayson 21:c279c6a83671 53 calibratedValues[s] = x;
DavidEGrayson 21:c279c6a83671 54 }
DavidEGrayson 21:c279c6a83671 55 }
DavidEGrayson 21:c279c6a83671 56
DavidEGrayson 21:c279c6a83671 57 void LineTracker::updateLineStatus()
DavidEGrayson 21:c279c6a83671 58 {
DavidEGrayson 21:c279c6a83671 59 uint32_t avg = 0;
DavidEGrayson 21:c279c6a83671 60 uint32_t sum = 0;
DavidEGrayson 21:c279c6a83671 61
DavidEGrayson 24:fc01d9125d3b 62 lineVisible = false;
DavidEGrayson 21:c279c6a83671 63 for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
DavidEGrayson 21:c279c6a83671 64 {
DavidEGrayson 21:c279c6a83671 65 // keep track of whether we see the line at all
DavidEGrayson 21:c279c6a83671 66 uint16_t value = calibratedValues[s];
DavidEGrayson 24:fc01d9125d3b 67 if (value > 500)
DavidEGrayson 21:c279c6a83671 68 {
DavidEGrayson 21:c279c6a83671 69 lineVisible = true;
DavidEGrayson 21:c279c6a83671 70 }
DavidEGrayson 21:c279c6a83671 71
DavidEGrayson 21:c279c6a83671 72 // only average in values that are above a noise threshold
DavidEGrayson 21:c279c6a83671 73 if (value > 50)
DavidEGrayson 21:c279c6a83671 74 {
DavidEGrayson 21:c279c6a83671 75 avg += (uint32_t)(value) * s * 1000;
DavidEGrayson 21:c279c6a83671 76 sum += value;
DavidEGrayson 21:c279c6a83671 77 }
DavidEGrayson 21:c279c6a83671 78 }
DavidEGrayson 21:c279c6a83671 79
DavidEGrayson 21:c279c6a83671 80 if (lineVisible)
DavidEGrayson 21:c279c6a83671 81 {
DavidEGrayson 21:c279c6a83671 82 linePosition = avg/sum;
DavidEGrayson 21:c279c6a83671 83 }
DavidEGrayson 21:c279c6a83671 84 else
DavidEGrayson 21:c279c6a83671 85 {
DavidEGrayson 21:c279c6a83671 86 // We cannot see the line, so just snap the position to the left-most or right-most
DavidEGrayson 21:c279c6a83671 87 // depending on what we saw previousl.
DavidEGrayson 21:c279c6a83671 88
DavidEGrayson 21:c279c6a83671 89 const uint32_t max = (LINE_SENSOR_COUNT-1)*1000;
DavidEGrayson 21:c279c6a83671 90 if(linePosition < max/2)
DavidEGrayson 21:c279c6a83671 91 {
DavidEGrayson 21:c279c6a83671 92 linePosition = 0;
DavidEGrayson 21:c279c6a83671 93 }
DavidEGrayson 21:c279c6a83671 94 else
DavidEGrayson 21:c279c6a83671 95 {
DavidEGrayson 21:c279c6a83671 96 linePosition = max;
DavidEGrayson 21:c279c6a83671 97 }
DavidEGrayson 21:c279c6a83671 98 }
DavidEGrayson 21:c279c6a83671 99 }
DavidEGrayson 21:c279c6a83671 100
DavidEGrayson 21:c279c6a83671 101 // The return value of this should only be heeded if the calibration seems to be OK.
DavidEGrayson 21:c279c6a83671 102 bool LineTracker::getLineVisible()
DavidEGrayson 21:c279c6a83671 103 {
DavidEGrayson 21:c279c6a83671 104 return lineVisible;
DavidEGrayson 21:c279c6a83671 105 }
DavidEGrayson 21:c279c6a83671 106
DavidEGrayson 21:c279c6a83671 107 uint16_t LineTracker::getLinePosition()
DavidEGrayson 21:c279c6a83671 108 {
DavidEGrayson 21:c279c6a83671 109 return linePosition;
DavidEGrayson 22:44c032e59ff5 110 }
DavidEGrayson 22:44c032e59ff5 111
DavidEGrayson 22:44c032e59ff5 112 void LineTracker::updateCalibration()
DavidEGrayson 22:44c032e59ff5 113 {
DavidEGrayson 22:44c032e59ff5 114 if(calibrationState == 0)
DavidEGrayson 22:44c032e59ff5 115 {
DavidEGrayson 22:44c032e59ff5 116 for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
DavidEGrayson 22:44c032e59ff5 117 {
DavidEGrayson 22:44c032e59ff5 118 recentValuesMin[s] = 0xFFFF;
DavidEGrayson 22:44c032e59ff5 119 recentValuesMax[s] = 0;
DavidEGrayson 22:44c032e59ff5 120 }
DavidEGrayson 22:44c032e59ff5 121 }
DavidEGrayson 22:44c032e59ff5 122
DavidEGrayson 22:44c032e59ff5 123 for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
DavidEGrayson 22:44c032e59ff5 124 {
DavidEGrayson 22:44c032e59ff5 125 uint16_t value = rawValues[s];
DavidEGrayson 22:44c032e59ff5 126 if (value < recentValuesMin[s]) { recentValuesMin[s] = value; }
DavidEGrayson 22:44c032e59ff5 127 if (value > recentValuesMax[s]) { recentValuesMax[s] = value; }
DavidEGrayson 22:44c032e59ff5 128 }
DavidEGrayson 22:44c032e59ff5 129
DavidEGrayson 22:44c032e59ff5 130 calibrationState = calibrationState + 1;
DavidEGrayson 22:44c032e59ff5 131
DavidEGrayson 22:44c032e59ff5 132 if (calibrationState == 9)
DavidEGrayson 22:44c032e59ff5 133 {
DavidEGrayson 22:44c032e59ff5 134 calibrationState = 0;
DavidEGrayson 22:44c032e59ff5 135
DavidEGrayson 22:44c032e59ff5 136 for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
DavidEGrayson 22:44c032e59ff5 137 {
DavidEGrayson 22:44c032e59ff5 138 if (recentValuesMin[s] > calibratedMaximum[s]) { calibratedMaximum[s] = recentValuesMin[s]; }
DavidEGrayson 22:44c032e59ff5 139 if (recentValuesMax[s] < calibratedMinimum[s]) { calibratedMinimum[s] = recentValuesMax[s]; }
DavidEGrayson 22:44c032e59ff5 140 }
DavidEGrayson 22:44c032e59ff5 141 }
DavidEGrayson 22:44c032e59ff5 142 }