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?
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 | 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 | } |