File content as of revision 21:c592bf6a6a2d:

// Eurobot13 Colour.cpp

#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)
{


    _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;
    }


}

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);

        //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 BLACK;
    }

}