ICRS Eurobot 2013
Dependencies: mbed mbed-rtos Servo QEI
Sensors/Colour/Colour.h
- Committer:
- twighk
- Date:
- 2013-04-01
- Revision:
- 3:717de74f6ebd
- Child:
- 4:1be0f6c6ceae
File content as of revision 3:717de74f6ebd:
// 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; } } };