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_sensors.cpp@37:23000a47ed2b, 2014-03-06 (annotated)
- Committer:
- DavidEGrayson
- Date:
- Thu Mar 06 05:11:46 2014 +0000
- Revision:
- 37:23000a47ed2b
- Parent:
- 32:83a13b06093c
- Child:
- 40:6fa672be85ec
With a boost 0.2% to handleRight in the Reckoner, this code did very well on the course twice!! Then I ran it the other way and it was more than a foot off :(
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 | 32:83a13b06093c | 13 | DigitalInOut(p18), // white wire, left-most sensor |
DavidEGrayson | 31:739b91331f31 | 14 | DigitalInOut(p19), // orange wire, middle sensor |
DavidEGrayson | 32:83a13b06093c | 15 | DigitalInOut(p20), // brown 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 | **/ |