David's line following code from the LVBots competition, 2015.
Dependencies: GeneralDebouncer Pacer PololuEncoder mbed
Fork of DeadReckoning by
line_sensors.cpp@31:739b91331f31, 2014-03-04 (annotated)
- Committer:
- DavidEGrayson
- Date:
- Tue Mar 04 03:04:00 2014 +0000
- Revision:
- 31:739b91331f31
- Parent:
- 29:cfcf08d8ac79
- Child:
- 32:83a13b06093c
testLineSensors seems to work fine with QTR-3RC.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
DavidEGrayson | 10:e4dd36148539 | 1 | #include "line_sensors.h" |
DavidEGrayson | 10:e4dd36148539 | 2 | |
DavidEGrayson | 31:739b91331f31 | 3 | |
DavidEGrayson | 31:739b91331f31 | 4 | /** |
DavidEGrayson | 10:e4dd36148539 | 5 | AnalogIn lineSensorsAnalog[LINE_SENSOR_COUNT] = { |
DavidEGrayson | 10:e4dd36148539 | 6 | AnalogIn(p20), // brown wire, left-most sensor |
DavidEGrayson | 10:e4dd36148539 | 7 | AnalogIn(p19), // orange wire, middle sensor |
DavidEGrayson | 28:4374035df5e0 | 8 | AnalogIn(p17), // blue wire, right-most sensor |
DavidEGrayson | 31:739b91331f31 | 9 | }; // TODO: remove |
DavidEGrayson | 31:739b91331f31 | 10 | **/ |
DavidEGrayson | 31:739b91331f31 | 11 | |
DavidEGrayson | 31:739b91331f31 | 12 | DigitalInOut lineSensorsDigital[LINE_SENSOR_COUNT] = { |
DavidEGrayson | 31:739b91331f31 | 13 | DigitalInOut(p18), // brown wire, left-most sensor |
DavidEGrayson | 31:739b91331f31 | 14 | DigitalInOut(p19), // orange wire, middle sensor |
DavidEGrayson | 31:739b91331f31 | 15 | DigitalInOut(p20), // blue wire, right-most sensor |
DavidEGrayson | 29:cfcf08d8ac79 | 16 | }; |
DavidEGrayson | 29:cfcf08d8ac79 | 17 | |
DavidEGrayson | 31:739b91331f31 | 18 | void readSensors(uint16_t * values) |
DavidEGrayson | 31:739b91331f31 | 19 | { |
DavidEGrayson | 31:739b91331f31 | 20 | for(uint8_t i = 0; i < LINE_SENSOR_COUNT; i++) |
DavidEGrayson | 31:739b91331f31 | 21 | { |
DavidEGrayson | 31:739b91331f31 | 22 | values[i] = 1000; |
DavidEGrayson | 31:739b91331f31 | 23 | lineSensorsDigital[i].mode(PullNone); |
DavidEGrayson | 31:739b91331f31 | 24 | lineSensorsDigital[i].output(); |
DavidEGrayson | 31:739b91331f31 | 25 | lineSensorsDigital[i].write(1); |
DavidEGrayson | 31:739b91331f31 | 26 | } |
DavidEGrayson | 31:739b91331f31 | 27 | |
DavidEGrayson | 31:739b91331f31 | 28 | wait_us(10); |
DavidEGrayson | 31:739b91331f31 | 29 | |
DavidEGrayson | 31:739b91331f31 | 30 | Timer timer; |
DavidEGrayson | 31:739b91331f31 | 31 | timer.start(); |
DavidEGrayson | 31:739b91331f31 | 32 | |
DavidEGrayson | 31:739b91331f31 | 33 | for(uint8_t i = 0; i < LINE_SENSOR_COUNT; i++) |
DavidEGrayson | 31:739b91331f31 | 34 | { |
DavidEGrayson | 31:739b91331f31 | 35 | lineSensorsDigital[i].input(); |
DavidEGrayson | 31:739b91331f31 | 36 | } |
DavidEGrayson | 31:739b91331f31 | 37 | |
DavidEGrayson | 31:739b91331f31 | 38 | while(timer.read_us() < 1000) |
DavidEGrayson | 31:739b91331f31 | 39 | { |
DavidEGrayson | 31:739b91331f31 | 40 | for(uint8_t i = 0; i < LINE_SENSOR_COUNT; i++) |
DavidEGrayson | 31:739b91331f31 | 41 | { |
DavidEGrayson | 31:739b91331f31 | 42 | if (values[i] == 1000 && lineSensorsDigital[i].read() == 0) |
DavidEGrayson | 31:739b91331f31 | 43 | { |
DavidEGrayson | 31:739b91331f31 | 44 | values[i] = timer.read_us(); |
DavidEGrayson | 31:739b91331f31 | 45 | } |
DavidEGrayson | 31:739b91331f31 | 46 | } |
DavidEGrayson | 31:739b91331f31 | 47 | } |
DavidEGrayson | 31:739b91331f31 | 48 | } |
DavidEGrayson | 31:739b91331f31 | 49 | |
DavidEGrayson | 31:739b91331f31 | 50 | |
DavidEGrayson | 29:cfcf08d8ac79 | 51 | /** |
DavidEGrayson | 29:cfcf08d8ac79 | 52 | uint16_t analogReadWithFilter(AnalogIn * input) |
DavidEGrayson | 29:cfcf08d8ac79 | 53 | { |
DavidEGrayson | 29:cfcf08d8ac79 | 54 | uint16_t readings[3]; |
DavidEGrayson | 29:cfcf08d8ac79 | 55 | for(uint8_t i = 0; i < 3; i++) |
DavidEGrayson | 29:cfcf08d8ac79 | 56 | { |
DavidEGrayson | 29:cfcf08d8ac79 | 57 | readings[i] = input->read_u16(); |
DavidEGrayson | 29:cfcf08d8ac79 | 58 | } |
DavidEGrayson | 29:cfcf08d8ac79 | 59 | |
DavidEGrayson | 29:cfcf08d8ac79 | 60 | if (readings[0] <= readings[1] && readings[0] >= readings[2]) |
DavidEGrayson | 29:cfcf08d8ac79 | 61 | { |
DavidEGrayson | 29:cfcf08d8ac79 | 62 | return readings[0]; |
DavidEGrayson | 29:cfcf08d8ac79 | 63 | } |
DavidEGrayson | 29:cfcf08d8ac79 | 64 | if (readings[1] <= readings[0] && readings[1] >= readings[2]) |
DavidEGrayson | 29:cfcf08d8ac79 | 65 | { |
DavidEGrayson | 29:cfcf08d8ac79 | 66 | return readings[1]; |
DavidEGrayson | 29:cfcf08d8ac79 | 67 | } |
DavidEGrayson | 29:cfcf08d8ac79 | 68 | return readings[2]; |
DavidEGrayson | 29:cfcf08d8ac79 | 69 | } |
DavidEGrayson | 29:cfcf08d8ac79 | 70 | **/ |