Diff: Sensors/Colour/Colour.cpp

Revision:

45:77cf6375348a

Parent:

43:c592bf6a6a2d

--- a/Sensors/Colour/Colour.cpp	Fri Apr 12 16:46:42 2013 +0000
+++ b/Sensors/Colour/Colour.cpp	Fri Apr 12 20:40:52 2013 +0000
@@ -3,96 +3,102 @@
 
 #include "Colour.h"
 #include "mbed.h"
-#include "math.h"
+
 
-Colour::Colour(PinName blue_led,
-               PinName red_led,
-               PinName pt,
-               ArmEnum arm)
-    : _blue_led(blue_led),
-      _red_led(red_led),
-      _pt(pt),
-      _arm(arm)
+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;
-    }
-    else if (arm == LOWER){
-    red_correction_factor = LOWERARM_CORRECTION;
-    }
-    else {
-    red_correction_factor = 0.00f;
+
+
+
+
+    if (arm == UPPER) {
+        red_correction_factor = UPPERARM_CORRECTION;
+    } else if (arm == LOWER) {
+        red_correction_factor = LOWERARM_CORRECTION;
+    } else {
+        red_correction_factor = 0.00f;
     }
 
+    togglecolour = 0;
+    blue = 0;
+    red = 0;
+    noise = 0;
+    buff_pointer = 0;
+
+
+    for (int i = 0; i < BUFF_SIZE; i++) {
+        blue_buff[i] = 0;
+        red_buff[i] = 0;
+        noise_buff[i] = 0;
+    }
+
+    ticker.attach(this, &Colour::Blink, 0.01);
 
 }
 
-void Colour::_Blink (void)
+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) {
+    if (togglecolour == 0) {
 
-        volatile float noise_temp = _pt.read();
+        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);
+        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);
+        float blue_base = (blue - noise);
+        float red_base = (red - noise)*red_correction_factor;
+        colour = atan2(red_base,blue_base);
 
         //toggles leds for the next state
-        _blue_led = 1;
-        _red_led = 0;
-    } else if (toggle_colour == 1) {
-        volatile float blue_temp = _pt.read();
+        blue_led = 1;
+        red_led = 0;
+    } else if (togglecolour == 1) {
+        float blue_temp = pt.read();
         blue += (blue_temp - blue_buff[buff_pointer])/BUFF_SIZE;
-        blue_buff[buff_pointer] = blue_temp;        
+        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();
+        blue_led = 0;
+        red_led = 1;
+    } else if (togglecolour == 2) {
+        float red_temp = pt.read();
         red += (red_temp - red_buff[buff_pointer])/BUFF_SIZE;
-        red_buff[buff_pointer] = red_temp;          
+        red_buff[buff_pointer] = red_temp;
         //toggles leds for the next state
-        _blue_led = 0;
-        _red_led = 0;
+        blue_led = 0;
+        red_led = 0;
     }
 
 
 
 
-    toggle_colour = (toggle_colour + 1) % 3;
+    togglecolour = (togglecolour + 1) % 3;
 
 
 }
 
 ColourEnum Colour::getColour()
 {
-    if (_SNR > SNR_THRESHOLD_DB) {
-        if ((_colour >= -30*PI/180) && (_colour < 30*PI/180)) {
+    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)) {
+        } else         if ((colour >= 30*PI/180) && (colour < 60*PI/180)) {
             return WHITE;
-        } else         if ((_colour >= 60*PI/180) && (_colour < 120*PI/180)) {
+        } else         if ((colour >= 60*PI/180) && (colour < 120*PI/180)) {
             return RED;
         } else {
             return BLACK;