David Grayson / Mbed 2 deprecated DeadReckoning

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@38:5e93a479c244, 2014-03-13 (annotated)

Committer:
DavidEGrayson
Date:
Thu Mar 13 17:49:43 2014 +0000
Revision:
38:5e93a479c244
Parent:
32:83a13b06093c
Child:
40:6fa672be85ec
The final version of the code that was used for the competition.;

Who changed what in which revision?

UserRevisionLine numberNew 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 **/