Diff: Sensors/Colour/Colour.cpp

Revision:

21:c592bf6a6a2d

Parent:

7:4340355261f9

Child:

45:77cf6375348a

--- a/Sensors/Colour/Colour.cpp	Tue Apr 09 15:33:36 2013 +0000
+++ b/Sensors/Colour/Colour.cpp	Fri Apr 12 16:24:25 2013 +0000
@@ -2,29 +2,103 @@
 // Eurobot13 Colour.cpp
 
 #include "Colour.h"
+#include "mbed.h"
+#include "math.h"
 
-void Colour::ReadLed (DigitalOut &led, float &avg, float &stdev, const int measureNum){
-    LedsOff();
-    led = 1;
-    double x = 0, x2 = 0;
-    for (int i = measureNum; i != 0; i--) {
-        float v = pt.read();
-        x += v;
-        x2+= v*v;
+Colour::Colour(PinName blue_led,
+               PinName red_led,
+               PinName pt,
+               ArmEnum arm)
+    : _blue_led(blue_led),
+      _red_led(red_led),
+      _pt(pt),
+      _arm(arm)
+{
+
+
+    _ticker.attach(this, &Colour::_Blink, 0.01);
+    
+    if (arm == UPPER){
+    red_correction_factor = UPPERARM_CORRECTION;
     }
-    avg = x / measureNum;
-    stdev = sqrt(x2 / measureNum - avg*avg);
-    LedsOff();
-    //pc.printf("Phototransistor Analog is: %f\t%f\n\r", avg, stdev);
+    else if (arm == LOWER){
+    red_correction_factor = LOWERARM_CORRECTION;
+    }
+    else {
+    red_correction_factor = 0.00f;
+    }
+
+
 }
 
-bool Colour::isColour(DigitalOut &led, const float &avg, const float &stdev, const float numstddev){
-    float avg2, stdev2;
-    ReadLed(led, avg2, stdev2);
+void Colour::_Blink (void)
+{
+    static int toggle_colour = 0;
+    static float blue = 0;
+    static float blue_buff[BUFF_SIZE];
+    static float red = 0;
+    static float red_buff[BUFF_SIZE];
+    static float noise = 0;
+    static float noise_buff[BUFF_SIZE];
+    
+    static int buff_pointer;
+
+    if (toggle_colour == 0) {
+
+        volatile float noise_temp = _pt.read();
+        noise += (noise_temp - noise_buff[buff_pointer])/BUFF_SIZE;
+        noise_buff[buff_pointer] = noise_temp;
+        
+        buff_pointer = (buff_pointer + 1) % BUFF_SIZE;
+
+
+        _SNR = 20.0f*log10(hypot(blue,red)/noise);
+
+        volatile double blue_base = (blue - noise);
+        volatile double red_base = (red - noise)*red_correction_factor;
+        _colour = atan2(red_base,blue_base);
 
-    if (avg + numstddev*stdev < avg2 - numstddev*stdev2) {
-        return true;
+        //toggles leds for the next state
+        _blue_led = 1;
+        _red_led = 0;
+    } else if (toggle_colour == 1) {
+        volatile float blue_temp = _pt.read();
+        blue += (blue_temp - blue_buff[buff_pointer])/BUFF_SIZE;
+        blue_buff[buff_pointer] = blue_temp;        
+        //toggles leds for the next state
+        _blue_led = 0;
+        _red_led = 1;
+    } else if (toggle_colour == 2) {
+        volatile float red_temp = _pt.read();
+        red += (red_temp - red_buff[buff_pointer])/BUFF_SIZE;
+        red_buff[buff_pointer] = red_temp;          
+        //toggles leds for the next state
+        _blue_led = 0;
+        _red_led = 0;
+    }
+
+
+
+
+    toggle_colour = (toggle_colour + 1) % 3;
+
+
+}
+
+ColourEnum Colour::getColour()
+{
+    if (_SNR > SNR_THRESHOLD_DB) {
+        if ((_colour >= -30*PI/180) && (_colour < 30*PI/180)) {
+            return BLUE;
+        } else         if ((_colour >= 30*PI/180) && (_colour < 60*PI/180)) {
+            return WHITE;
+        } else         if ((_colour >= 60*PI/180) && (_colour < 120*PI/180)) {
+            return RED;
+        } else {
+            return BLACK;
+        }
     } else {
-        return false;
+        return BLACK;
     }
-}
\ No newline at end of file
+
+}