This is some awesome robot code
Dependencies: mbed-rtos mbed QEI
Fork of ICRSEurobot13 by
Diff: Sensors/Colour/Colour.cpp
- Revision:
- 43: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 + +}