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@21:c279c6a83671, 2014-02-27 (annotated)

Committer:
DavidEGrayson
Date:
Thu Feb 27 23:20:34 2014 +0000
Revision:
21:c279c6a83671
Child:
22:44c032e59ff5
Wrote a whole bunch of code that could theoretically allow the robot to compete, but it has not been tested at all yet.

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