File content as of revision 46:adcd57a5e402:

// Eurobot13 Colour.cpp

#include "Colour.h"
#include "mbed.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)
{






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


    if (togglecolour == 0) {

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

        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 (togglecolour == 1) {
        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 (togglecolour == 2) {
        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;
    }




    togglecolour = (togglecolour + 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;
    }

}