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@31:739b91331f31, 2014-03-04 (annotated)
- Committer:
- DavidEGrayson
- Date:
- Tue Mar 04 03:04:00 2014 +0000
- Revision:
- 31:739b91331f31
- Parent:
- 24:fc01d9125d3b
- Child:
- 46:df2c2d25c070
testLineSensors seems to work fine with QTR-3RC.
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 | 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 | 21:c279c6a83671 | 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 | 21:c279c6a83671 | 33 | if(denominator != 0) |
DavidEGrayson | 21:c279c6a83671 | 34 | { |
DavidEGrayson | 21:c279c6a83671 | 35 | x = ((int32_t)rawValues[s] - calmin) * 1000 / denominator; |
DavidEGrayson | 21:c279c6a83671 | 36 | if(x < 0) |
DavidEGrayson | 21:c279c6a83671 | 37 | { |
DavidEGrayson | 21:c279c6a83671 | 38 | x = 0; |
DavidEGrayson | 21:c279c6a83671 | 39 | } |
DavidEGrayson | 21:c279c6a83671 | 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 | 21:c279c6a83671 | 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 | 21:c279c6a83671 | 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 | 21:c279c6a83671 | 81 | const uint32_t max = (LINE_SENSOR_COUNT-1)*1000; |
DavidEGrayson | 21:c279c6a83671 | 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 | } |