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@48:597738b77f77, 2019-08-13 (annotated)

Committer:
DavidEGrayson
Date:
Tue Aug 13 21:21:17 2019 +0000
Revision:
48:597738b77f77
Parent:
46:df2c2d25c070 
Changes from before the contest, I think.

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