This is some awesome robot code
Dependencies: mbed-rtos mbed QEI
Fork of ICRSEurobot13 by
Diff: Sensors/Colour/Colour.cpp
- Revision:
- 45:77cf6375348a
- Parent:
- 43:c592bf6a6a2d
diff -r 555136de33e4 -r 77cf6375348a Sensors/Colour/Colour.cpp --- 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;