David's line following code from the LVBots competition, 2015.
Dependencies: GeneralDebouncer Pacer PololuEncoder mbed
Fork of DeadReckoning by
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?
User | Revision | Line number | New 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 | } |