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@22:44c032e59ff5, 2014-02-28 (annotated)

Committer:
DavidEGrayson
Date:
Fri Feb 28 00:16:49 2014 +0000
Revision:
22:44c032e59ff5
Parent:
21:c279c6a83671
Child:
24:fc01d9125d3b
Fixed the code for calibrating.  Added testCalibrate.

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 21:c279c6a83671 62 for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
DavidEGrayson 21:c279c6a83671 63 {
DavidEGrayson 21:c279c6a83671 64 // keep track of whether we see the line at all
DavidEGrayson 21:c279c6a83671 65 uint16_t value = calibratedValues[s];
DavidEGrayson 21:c279c6a83671 66 if (value > 200)
DavidEGrayson 21:c279c6a83671 67 {
DavidEGrayson 21:c279c6a83671 68 lineVisible = true;
DavidEGrayson 21:c279c6a83671 69 }
DavidEGrayson 21:c279c6a83671 70
DavidEGrayson 21:c279c6a83671 71 // only average in values that are above a noise threshold
DavidEGrayson 21:c279c6a83671 72 if (value > 50)
DavidEGrayson 21:c279c6a83671 73 {
DavidEGrayson 21:c279c6a83671 74 avg += (uint32_t)(value) * s * 1000;
DavidEGrayson 21:c279c6a83671 75 sum += value;
DavidEGrayson 21:c279c6a83671 76 }
DavidEGrayson 21:c279c6a83671 77 }
DavidEGrayson 21:c279c6a83671 78
DavidEGrayson 21:c279c6a83671 79 if (lineVisible)
DavidEGrayson 21:c279c6a83671 80 {
DavidEGrayson 21:c279c6a83671 81 linePosition = avg/sum;
DavidEGrayson 21:c279c6a83671 82 }
DavidEGrayson 21:c279c6a83671 83 else
DavidEGrayson 21:c279c6a83671 84 {
DavidEGrayson 21:c279c6a83671 85 // We cannot see the line, so just snap the position to the left-most or right-most
DavidEGrayson 21:c279c6a83671 86 // depending on what we saw previousl.
DavidEGrayson 21:c279c6a83671 87
DavidEGrayson 21:c279c6a83671 88 const uint32_t max = (LINE_SENSOR_COUNT-1)*1000;
DavidEGrayson 21:c279c6a83671 89 if(linePosition < max/2)
DavidEGrayson 21:c279c6a83671 90 {
DavidEGrayson 21:c279c6a83671 91 linePosition = 0;
DavidEGrayson 21:c279c6a83671 92 }
DavidEGrayson 21:c279c6a83671 93 else
DavidEGrayson 21:c279c6a83671 94 {
DavidEGrayson 21:c279c6a83671 95 linePosition = max;
DavidEGrayson 21:c279c6a83671 96 }
DavidEGrayson 21:c279c6a83671 97 }
DavidEGrayson 21:c279c6a83671 98 }
DavidEGrayson 21:c279c6a83671 99
DavidEGrayson 21:c279c6a83671 100 // The return value of this should only be heeded if the calibration seems to be OK.
DavidEGrayson 21:c279c6a83671 101 bool LineTracker::getLineVisible()
DavidEGrayson 21:c279c6a83671 102 {
DavidEGrayson 21:c279c6a83671 103 return lineVisible;
DavidEGrayson 21:c279c6a83671 104 }
DavidEGrayson 21:c279c6a83671 105
DavidEGrayson 21:c279c6a83671 106 uint16_t LineTracker::getLinePosition()
DavidEGrayson 21:c279c6a83671 107 {
DavidEGrayson 21:c279c6a83671 108 return linePosition;
DavidEGrayson 22:44c032e59ff5 109 }
DavidEGrayson 22:44c032e59ff5 110
DavidEGrayson 22:44c032e59ff5 111 void LineTracker::updateCalibration()
DavidEGrayson 22:44c032e59ff5 112 {
DavidEGrayson 22:44c032e59ff5 113 if(calibrationState == 0)
DavidEGrayson 22:44c032e59ff5 114 {
DavidEGrayson 22:44c032e59ff5 115 for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
DavidEGrayson 22:44c032e59ff5 116 {
DavidEGrayson 22:44c032e59ff5 117 recentValuesMin[s] = 0xFFFF;
DavidEGrayson 22:44c032e59ff5 118 recentValuesMax[s] = 0;
DavidEGrayson 22:44c032e59ff5 119 }
DavidEGrayson 22:44c032e59ff5 120 }
DavidEGrayson 22:44c032e59ff5 121
DavidEGrayson 22:44c032e59ff5 122 for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
DavidEGrayson 22:44c032e59ff5 123 {
DavidEGrayson 22:44c032e59ff5 124 uint16_t value = rawValues[s];
DavidEGrayson 22:44c032e59ff5 125 if (value < recentValuesMin[s]) { recentValuesMin[s] = value; }
DavidEGrayson 22:44c032e59ff5 126 if (value > recentValuesMax[s]) { recentValuesMax[s] = value; }
DavidEGrayson 22:44c032e59ff5 127 }
DavidEGrayson 22:44c032e59ff5 128
DavidEGrayson 22:44c032e59ff5 129 calibrationState = calibrationState + 1;
DavidEGrayson 22:44c032e59ff5 130
DavidEGrayson 22:44c032e59ff5 131 if (calibrationState == 9)
DavidEGrayson 22:44c032e59ff5 132 {
DavidEGrayson 22:44c032e59ff5 133 calibrationState = 0;
DavidEGrayson 22:44c032e59ff5 134
DavidEGrayson 22:44c032e59ff5 135 for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
DavidEGrayson 22:44c032e59ff5 136 {
DavidEGrayson 22:44c032e59ff5 137 if (recentValuesMin[s] > calibratedMaximum[s]) { calibratedMaximum[s] = recentValuesMin[s]; }
DavidEGrayson 22:44c032e59ff5 138 if (recentValuesMax[s] < calibratedMinimum[s]) { calibratedMinimum[s] = recentValuesMax[s]; }
DavidEGrayson 22:44c032e59ff5 139 }
DavidEGrayson 22:44c032e59ff5 140 }
DavidEGrayson 22:44c032e59ff5 141 }