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