zhouhang shao
/
test
test
IRSensor.h@6:9f698d1b2996, 2017-05-13 (annotated)
- Committer:
- kolanery
- Date:
- Sat May 13 19:42:23 2017 +0000
- Revision:
- 6:9f698d1b2996
- Parent:
- 1:11970e541ecf
update test case
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
szh66 | 1:11970e541ecf | 1 | #ifndef IRSENSOR_H |
szh66 | 1:11970e541ecf | 2 | #define IRSENSOR_H |
szh66 | 0:3c59bc5c9388 | 3 | |
szh66 | 0:3c59bc5c9388 | 4 | #include "mbed.h" |
szh66 | 0:3c59bc5c9388 | 5 | |
szh66 | 0:3c59bc5c9388 | 6 | extern Serial pc; |
szh66 | 0:3c59bc5c9388 | 7 | |
szh66 | 0:3c59bc5c9388 | 8 | class IRSensor { |
szh66 | 0:3c59bc5c9388 | 9 | public: |
szh66 | 0:3c59bc5c9388 | 10 | DigitalOut _enable; |
szh66 | 0:3c59bc5c9388 | 11 | AnalogIn _input; |
szh66 | 0:3c59bc5c9388 | 12 | volatile float value; |
szh66 | 0:3c59bc5c9388 | 13 | float COEFF_1; |
szh66 | 0:3c59bc5c9388 | 14 | float COEFF_2; |
szh66 | 0:3c59bc5c9388 | 15 | float COEFF_3; |
szh66 | 0:3c59bc5c9388 | 16 | float COEFF_4; |
szh66 | 0:3c59bc5c9388 | 17 | float COEFF_5; |
szh66 | 0:3c59bc5c9388 | 18 | float COEFF_6; |
szh66 | 0:3c59bc5c9388 | 19 | float COEFF_7; |
szh66 | 0:3c59bc5c9388 | 20 | |
szh66 | 0:3c59bc5c9388 | 21 | float last_read[5]; |
szh66 | 0:3c59bc5c9388 | 22 | |
szh66 | 0:3c59bc5c9388 | 23 | IRSensor(PinName enable, PinName input, float c1 = 0.217722944294793f, float c2 = -5.50965509541424f, |
szh66 | 0:3c59bc5c9388 | 24 | float c3 = 55.0111470003071f, float c4 = -229.934241342364f, float c5 = 506.986371752309f, |
szh66 | 0:3c59bc5c9388 | 25 | float c6 = -502.862134567129f, float c7 = 177.712915019135f) : _enable(enable), _input(input), |
szh66 | 0:3c59bc5c9388 | 26 | COEFF_1(c1), COEFF_2(c2), COEFF_3(c3), COEFF_4(c4), COEFF_5(c5), COEFF_6(c6), COEFF_7(c7) { |
szh66 | 0:3c59bc5c9388 | 27 | } |
szh66 | 0:3c59bc5c9388 | 28 | |
szh66 | 0:3c59bc5c9388 | 29 | //Get the value in encoder units, samples?... |
szh66 | 0:3c59bc5c9388 | 30 | float readIR(){ |
szh66 | 0:3c59bc5c9388 | 31 | float sum = 0; |
szh66 | 0:3c59bc5c9388 | 32 | |
szh66 | 1:11970e541ecf | 33 | |
szh66 | 0:3c59bc5c9388 | 34 | //Each duration takes 100us, 5 times = 0.5ms |
szh66 | 0:3c59bc5c9388 | 35 | for (int i = 0; i < 5; i++) |
szh66 | 0:3c59bc5c9388 | 36 | { |
szh66 | 0:3c59bc5c9388 | 37 | //Turn on IR LED tx |
szh66 | 0:3c59bc5c9388 | 38 | _enable = 1; |
szh66 | 0:3c59bc5c9388 | 39 | |
szh66 | 0:3c59bc5c9388 | 40 | //Wait for capacitor to fire, 10us |
szh66 | 0:3c59bc5c9388 | 41 | wait_us(6); |
szh66 | 0:3c59bc5c9388 | 42 | |
szh66 | 0:3c59bc5c9388 | 43 | last_read[i] = _input.read(); |
szh66 | 0:3c59bc5c9388 | 44 | if (i){ |
szh66 | 0:3c59bc5c9388 | 45 | sum += last_read[i]; |
szh66 | 0:3c59bc5c9388 | 46 | } |
szh66 | 0:3c59bc5c9388 | 47 | //Wait 5us for turning off IR LED tx |
szh66 | 0:3c59bc5c9388 | 48 | _enable = 0; |
szh66 | 0:3c59bc5c9388 | 49 | |
szh66 | 0:3c59bc5c9388 | 50 | //Wait 85us for turning on IR LED tx |
szh66 | 0:3c59bc5c9388 | 51 | wait_us(75); |
szh66 | 0:3c59bc5c9388 | 52 | } |
szh66 | 0:3c59bc5c9388 | 53 | |
szh66 | 0:3c59bc5c9388 | 54 | sum /= 4; |
szh66 | 0:3c59bc5c9388 | 55 | value = sum; |
szh66 | 0:3c59bc5c9388 | 56 | |
szh66 | 0:3c59bc5c9388 | 57 | float square = sum * sum; |
szh66 | 0:3c59bc5c9388 | 58 | float cube = square * sum; |
szh66 | 0:3c59bc5c9388 | 59 | float _value = 0; |
szh66 | 0:3c59bc5c9388 | 60 | _value += COEFF_1 / cube; |
szh66 | 0:3c59bc5c9388 | 61 | _value += COEFF_2 / square; |
szh66 | 0:3c59bc5c9388 | 62 | _value += COEFF_3 / sum; |
szh66 | 0:3c59bc5c9388 | 63 | _value += COEFF_4; |
szh66 | 0:3c59bc5c9388 | 64 | _value += COEFF_5 * sum; |
szh66 | 0:3c59bc5c9388 | 65 | _value += COEFF_6 * square; |
szh66 | 0:3c59bc5c9388 | 66 | _value += COEFF_7 * square * sum; |
szh66 | 0:3c59bc5c9388 | 67 | |
szh66 | 1:11970e541ecf | 68 | return value; |
szh66 | 0:3c59bc5c9388 | 69 | } |
szh66 | 0:3c59bc5c9388 | 70 | |
szh66 | 0:3c59bc5c9388 | 71 | //Get the number of cells away wall is |
szh66 | 0:3c59bc5c9388 | 72 | float cell_dist(){ |
szh66 | 0:3c59bc5c9388 | 73 | return readIR() / 18; |
szh66 | 0:3c59bc5c9388 | 74 | } |
szh66 | 0:3c59bc5c9388 | 75 | |
szh66 | 0:3c59bc5c9388 | 76 | //Shorthand for read() |
szh66 | 0:3c59bc5c9388 | 77 | operator float() { |
szh66 | 0:3c59bc5c9388 | 78 | return readIR(); |
szh66 | 0:3c59bc5c9388 | 79 | } |
szh66 | 0:3c59bc5c9388 | 80 | |
szh66 | 0:3c59bc5c9388 | 81 | }; |
szh66 | 0:3c59bc5c9388 | 82 | |
szh66 | 0:3c59bc5c9388 | 83 | extern IRSensor rightIR; |
szh66 | 0:3c59bc5c9388 | 84 | extern IRSensor rightDiagonalIR; |
szh66 | 0:3c59bc5c9388 | 85 | extern IRSensor rightFrontIR; |
szh66 | 0:3c59bc5c9388 | 86 | extern IRSensor leftFrontIR; |
szh66 | 0:3c59bc5c9388 | 87 | extern IRSensor leftDiagonalIR; |
szh66 | 0:3c59bc5c9388 | 88 | extern IRSensor leftIR; |
szh66 | 0:3c59bc5c9388 | 89 | |
szh66 | 0:3c59bc5c9388 | 90 | |
szh66 | 0:3c59bc5c9388 | 91 | #endif |