David's line following code from the LVBots competition, 2015.
Dependencies: GeneralDebouncer Pacer PololuEncoder mbed
Fork of DeadReckoning by
line_sensors.cpp@47:cb5c1504c24d, 2015-04-15 (annotated)
- Committer:
- DavidEGrayson
- Date:
- Wed Apr 15 21:19:52 2015 +0000
- Revision:
- 47:cb5c1504c24d
- Parent:
- 40:e79cefc241f8
fixed something in logger;
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 | DigitalInOut lineSensorsDigital[LINE_SENSOR_COUNT] = { |
DavidEGrayson | 32:83a13b06093c | 4 | DigitalInOut(p18), // white wire, left-most sensor |
DavidEGrayson | 31:739b91331f31 | 5 | DigitalInOut(p19), // orange wire, middle sensor |
DavidEGrayson | 32:83a13b06093c | 6 | DigitalInOut(p20), // brown wire, right-most sensor |
DavidEGrayson | 29:cfcf08d8ac79 | 7 | }; |
DavidEGrayson | 29:cfcf08d8ac79 | 8 | |
DavidEGrayson | 31:739b91331f31 | 9 | void readSensors(uint16_t * values) |
DavidEGrayson | 31:739b91331f31 | 10 | { |
DavidEGrayson | 31:739b91331f31 | 11 | for(uint8_t i = 0; i < LINE_SENSOR_COUNT; i++) |
DavidEGrayson | 31:739b91331f31 | 12 | { |
DavidEGrayson | 31:739b91331f31 | 13 | values[i] = 1000; |
DavidEGrayson | 31:739b91331f31 | 14 | lineSensorsDigital[i].mode(PullNone); |
DavidEGrayson | 31:739b91331f31 | 15 | lineSensorsDigital[i].output(); |
DavidEGrayson | 31:739b91331f31 | 16 | lineSensorsDigital[i].write(1); |
DavidEGrayson | 31:739b91331f31 | 17 | } |
DavidEGrayson | 31:739b91331f31 | 18 | |
DavidEGrayson | 31:739b91331f31 | 19 | wait_us(10); |
DavidEGrayson | 31:739b91331f31 | 20 | |
DavidEGrayson | 31:739b91331f31 | 21 | for(uint8_t i = 0; i < LINE_SENSOR_COUNT; i++) |
DavidEGrayson | 31:739b91331f31 | 22 | { |
DavidEGrayson | 31:739b91331f31 | 23 | lineSensorsDigital[i].input(); |
DavidEGrayson | 31:739b91331f31 | 24 | } |
DavidEGrayson | 31:739b91331f31 | 25 | |
DavidEGrayson | 40:e79cefc241f8 | 26 | Timer timer; |
DavidEGrayson | 40:e79cefc241f8 | 27 | timer.start(); |
DavidEGrayson | 40:e79cefc241f8 | 28 | |
DavidEGrayson | 31:739b91331f31 | 29 | while(timer.read_us() < 1000) |
DavidEGrayson | 31:739b91331f31 | 30 | { |
DavidEGrayson | 31:739b91331f31 | 31 | for(uint8_t i = 0; i < LINE_SENSOR_COUNT; i++) |
DavidEGrayson | 31:739b91331f31 | 32 | { |
DavidEGrayson | 31:739b91331f31 | 33 | if (values[i] == 1000 && lineSensorsDigital[i].read() == 0) |
DavidEGrayson | 31:739b91331f31 | 34 | { |
DavidEGrayson | 31:739b91331f31 | 35 | values[i] = timer.read_us(); |
DavidEGrayson | 31:739b91331f31 | 36 | } |
DavidEGrayson | 31:739b91331f31 | 37 | } |
DavidEGrayson | 31:739b91331f31 | 38 | } |
DavidEGrayson | 31:739b91331f31 | 39 | } |
DavidEGrayson | 31:739b91331f31 | 40 | |
DavidEGrayson | 31:739b91331f31 | 41 | |
DavidEGrayson | 29:cfcf08d8ac79 | 42 | /** |
DavidEGrayson | 29:cfcf08d8ac79 | 43 | uint16_t analogReadWithFilter(AnalogIn * input) |
DavidEGrayson | 29:cfcf08d8ac79 | 44 | { |
DavidEGrayson | 29:cfcf08d8ac79 | 45 | uint16_t readings[3]; |
DavidEGrayson | 29:cfcf08d8ac79 | 46 | for(uint8_t i = 0; i < 3; i++) |
DavidEGrayson | 29:cfcf08d8ac79 | 47 | { |
DavidEGrayson | 29:cfcf08d8ac79 | 48 | readings[i] = input->read_u16(); |
DavidEGrayson | 29:cfcf08d8ac79 | 49 | } |
DavidEGrayson | 29:cfcf08d8ac79 | 50 | |
DavidEGrayson | 29:cfcf08d8ac79 | 51 | if (readings[0] <= readings[1] && readings[0] >= readings[2]) |
DavidEGrayson | 29:cfcf08d8ac79 | 52 | { |
DavidEGrayson | 29:cfcf08d8ac79 | 53 | return readings[0]; |
DavidEGrayson | 29:cfcf08d8ac79 | 54 | } |
DavidEGrayson | 29:cfcf08d8ac79 | 55 | if (readings[1] <= readings[0] && readings[1] >= readings[2]) |
DavidEGrayson | 29:cfcf08d8ac79 | 56 | { |
DavidEGrayson | 29:cfcf08d8ac79 | 57 | return readings[1]; |
DavidEGrayson | 29:cfcf08d8ac79 | 58 | } |
DavidEGrayson | 29:cfcf08d8ac79 | 59 | return readings[2]; |
DavidEGrayson | 29:cfcf08d8ac79 | 60 | } |
DavidEGrayson | 29:cfcf08d8ac79 | 61 | **/ |