ICRS Eurobot 2013
Dependencies: mbed mbed-rtos Servo QEI
Diff: Sensors/Colour/Colour.h
- Revision:
- 3:717de74f6ebd
- Child:
- 4:1be0f6c6ceae
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Sensors/Colour/Colour.h Mon Apr 01 15:33:48 2013 +0000 @@ -0,0 +1,76 @@ + +// Eurobot13 Colour.h + + +#include "mbed.h" +#include "Led.h" +#include "Phototransistor.h" + +enum ColourEnum {BLUE, RED, WHITE, INCONCLUSIVE, BUG}; + +class Colour{ +private: + Led blue; float bavg, bstdev; + Led red; float ravg, rstdev; + Phototransistor pt; + +public: + Colour(PinName bluePin, PinName redPin, PinName phototransistorPin) + : blue(bluePin) + , red (redPin) + , pt (phototransistorPin) + { + LedsOff(); + } + + void Calibrate(){ + ReadLed(blue, bavg, bstdev); + ReadLed( red, ravg, rstdev); + } + + ColourEnum getColour(){ + bool blueb = isColour(blue, bavg, bstdev) + , redb = isColour( red, ravg, rstdev); + + if ( blueb && redb) + {return WHITE;} + else if ( blueb && !redb) + {return BLUE;} + else if (!blueb && redb) + {return RED;} + else if (!blueb && !redb) + {return INCONCLUSIVE;} + return BUG; + } + +private: + void LedsOff(){blue.off(); red.off();} + void ReadLed (Led &led, float &avg, float &stdev, const int measureNum = 25){ + LedsOff(); led.on(); + double x = 0, x2 = 0; + for (int i = measureNum; i != 0; i--){ + float v = pt.read(); + x += v; + x2+= v*v; + } + avg = x / measureNum; + stdev = sqrt(x2 / measureNum - avg*avg); + LedsOff(); + + pc.printf("Phototransistor Analog is: %f\t%f\n\r", avg, stdev); + } + + bool isColour(Led &led, const float &avg, const float &stdev, const float numstddev = 2 ){ + float avg2, stdev2; + ReadLed(led, avg2, stdev2); + + if (avg + numstddev*stdev < avg2 - numstddev*stdev2){ + return true; + } else { + return false; + } + } + + + +}; \ No newline at end of file