This is some awesome robot code
Dependencies: mbed-rtos mbed QEI
Fork of ICRSEurobot13 by
Sensors/Colour/Colour.h@3:717de74f6ebd, 2013-04-01 (annotated)
- Committer:
- twighk
- Date:
- Mon Apr 01 15:33:48 2013 +0000
- Revision:
- 3:717de74f6ebd
- Child:
- 4:1be0f6c6ceae
Colour sensor, red SnR too Low
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
twighk | 3:717de74f6ebd | 1 | |
twighk | 3:717de74f6ebd | 2 | // Eurobot13 Colour.h |
twighk | 3:717de74f6ebd | 3 | |
twighk | 3:717de74f6ebd | 4 | |
twighk | 3:717de74f6ebd | 5 | #include "mbed.h" |
twighk | 3:717de74f6ebd | 6 | #include "Led.h" |
twighk | 3:717de74f6ebd | 7 | #include "Phototransistor.h" |
twighk | 3:717de74f6ebd | 8 | |
twighk | 3:717de74f6ebd | 9 | enum ColourEnum {BLUE, RED, WHITE, INCONCLUSIVE, BUG}; |
twighk | 3:717de74f6ebd | 10 | |
twighk | 3:717de74f6ebd | 11 | class Colour{ |
twighk | 3:717de74f6ebd | 12 | private: |
twighk | 3:717de74f6ebd | 13 | Led blue; float bavg, bstdev; |
twighk | 3:717de74f6ebd | 14 | Led red; float ravg, rstdev; |
twighk | 3:717de74f6ebd | 15 | Phototransistor pt; |
twighk | 3:717de74f6ebd | 16 | |
twighk | 3:717de74f6ebd | 17 | public: |
twighk | 3:717de74f6ebd | 18 | Colour(PinName bluePin, PinName redPin, PinName phototransistorPin) |
twighk | 3:717de74f6ebd | 19 | : blue(bluePin) |
twighk | 3:717de74f6ebd | 20 | , red (redPin) |
twighk | 3:717de74f6ebd | 21 | , pt (phototransistorPin) |
twighk | 3:717de74f6ebd | 22 | { |
twighk | 3:717de74f6ebd | 23 | LedsOff(); |
twighk | 3:717de74f6ebd | 24 | } |
twighk | 3:717de74f6ebd | 25 | |
twighk | 3:717de74f6ebd | 26 | void Calibrate(){ |
twighk | 3:717de74f6ebd | 27 | ReadLed(blue, bavg, bstdev); |
twighk | 3:717de74f6ebd | 28 | ReadLed( red, ravg, rstdev); |
twighk | 3:717de74f6ebd | 29 | } |
twighk | 3:717de74f6ebd | 30 | |
twighk | 3:717de74f6ebd | 31 | ColourEnum getColour(){ |
twighk | 3:717de74f6ebd | 32 | bool blueb = isColour(blue, bavg, bstdev) |
twighk | 3:717de74f6ebd | 33 | , redb = isColour( red, ravg, rstdev); |
twighk | 3:717de74f6ebd | 34 | |
twighk | 3:717de74f6ebd | 35 | if ( blueb && redb) |
twighk | 3:717de74f6ebd | 36 | {return WHITE;} |
twighk | 3:717de74f6ebd | 37 | else if ( blueb && !redb) |
twighk | 3:717de74f6ebd | 38 | {return BLUE;} |
twighk | 3:717de74f6ebd | 39 | else if (!blueb && redb) |
twighk | 3:717de74f6ebd | 40 | {return RED;} |
twighk | 3:717de74f6ebd | 41 | else if (!blueb && !redb) |
twighk | 3:717de74f6ebd | 42 | {return INCONCLUSIVE;} |
twighk | 3:717de74f6ebd | 43 | return BUG; |
twighk | 3:717de74f6ebd | 44 | } |
twighk | 3:717de74f6ebd | 45 | |
twighk | 3:717de74f6ebd | 46 | private: |
twighk | 3:717de74f6ebd | 47 | void LedsOff(){blue.off(); red.off();} |
twighk | 3:717de74f6ebd | 48 | void ReadLed (Led &led, float &avg, float &stdev, const int measureNum = 25){ |
twighk | 3:717de74f6ebd | 49 | LedsOff(); led.on(); |
twighk | 3:717de74f6ebd | 50 | double x = 0, x2 = 0; |
twighk | 3:717de74f6ebd | 51 | for (int i = measureNum; i != 0; i--){ |
twighk | 3:717de74f6ebd | 52 | float v = pt.read(); |
twighk | 3:717de74f6ebd | 53 | x += v; |
twighk | 3:717de74f6ebd | 54 | x2+= v*v; |
twighk | 3:717de74f6ebd | 55 | } |
twighk | 3:717de74f6ebd | 56 | avg = x / measureNum; |
twighk | 3:717de74f6ebd | 57 | stdev = sqrt(x2 / measureNum - avg*avg); |
twighk | 3:717de74f6ebd | 58 | LedsOff(); |
twighk | 3:717de74f6ebd | 59 | |
twighk | 3:717de74f6ebd | 60 | pc.printf("Phototransistor Analog is: %f\t%f\n\r", avg, stdev); |
twighk | 3:717de74f6ebd | 61 | } |
twighk | 3:717de74f6ebd | 62 | |
twighk | 3:717de74f6ebd | 63 | bool isColour(Led &led, const float &avg, const float &stdev, const float numstddev = 2 ){ |
twighk | 3:717de74f6ebd | 64 | float avg2, stdev2; |
twighk | 3:717de74f6ebd | 65 | ReadLed(led, avg2, stdev2); |
twighk | 3:717de74f6ebd | 66 | |
twighk | 3:717de74f6ebd | 67 | if (avg + numstddev*stdev < avg2 - numstddev*stdev2){ |
twighk | 3:717de74f6ebd | 68 | return true; |
twighk | 3:717de74f6ebd | 69 | } else { |
twighk | 3:717de74f6ebd | 70 | return false; |
twighk | 3:717de74f6ebd | 71 | } |
twighk | 3:717de74f6ebd | 72 | } |
twighk | 3:717de74f6ebd | 73 | |
twighk | 3:717de74f6ebd | 74 | |
twighk | 3:717de74f6ebd | 75 | |
twighk | 3:717de74f6ebd | 76 | }; |