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@27:2456f68be679, 2014-03-01 (annotated)

Committer:
DavidEGrayson
Date:
Sat Mar 01 01:46:35 2014 +0000
Revision:
27:2456f68be679
Parent:
24:fc01d9125d3b
Child:
31:739b91331f31
Fixed a major bug in the line following (reduceSpeed return value was not used).  Made finalSettleIn better by adding an integral term and increasing the settleSpeed from 200 to 300.

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 }