Colour.cpp

00001 
00002 // Eurobot13 Colour.cpp
00003 
00004 #include "Colour.h"
00005 #include "mbed.h"
00006 
00007 
00008 Colour::Colour(PinName _blue_led,
00009                PinName _red_led,
00010                PinName _pt,
00011                ArmEnum _arm)
00012     : blue_led(_blue_led),
00013       red_led(_red_led),
00014       pt(_pt),
00015       arm(_arm)
00016 {
00017 
00018 
00019 
00020 
00021 
00022 
00023     if (arm == UPPER) {
00024         red_correction_factor = UPPERARM_CORRECTION;
00025     } else if (arm == LOWER) {
00026         red_correction_factor = LOWERARM_CORRECTION;
00027     } else {
00028         red_correction_factor = 0.00f;
00029     }
00030 
00031     togglecolour = 0;
00032     blue = 0;
00033     red = 0;
00034     noise = 0;
00035     buff_pointer = 0;
00036 
00037 
00038     for (int i = 0; i < BUFF_SIZE; i++) {
00039         blue_buff[i] = 0;
00040         red_buff[i] = 0;
00041         noise_buff[i] = 0;
00042     }
00043 
00044     ticker.attach(this, &Colour::Blink, 0.01);
00045 
00046 }
00047 
00048 void Colour::Blink (void)
00049 {
00050 
00051 
00052     if (togglecolour == 0) {
00053 
00054         float noise_temp = pt.read();
00055         noise += (noise_temp - noise_buff[buff_pointer])/BUFF_SIZE;
00056         noise_buff[buff_pointer] = noise_temp;
00057 
00058         buff_pointer = (buff_pointer + 1) % BUFF_SIZE;
00059 
00060 
00061         SNR = 20.0f*log10(hypot(blue,red)/noise);
00062 
00063         float blue_base = (blue - noise);
00064         float red_base = (red - noise)*red_correction_factor;
00065         colour = atan2(red_base,blue_base);
00066 
00067         //toggles leds for the next state
00068         blue_led = 1;
00069         red_led = 0;
00070     } else if (togglecolour == 1) {
00071         float blue_temp = pt.read();
00072         blue += (blue_temp - blue_buff[buff_pointer])/BUFF_SIZE;
00073         blue_buff[buff_pointer] = blue_temp;
00074         //toggles leds for the next state
00075         blue_led = 0;
00076         red_led = 1;
00077     } else if (togglecolour == 2) {
00078         float red_temp = pt.read();
00079         red += (red_temp - red_buff[buff_pointer])/BUFF_SIZE;
00080         red_buff[buff_pointer] = red_temp;
00081         //toggles leds for the next state
00082         blue_led = 0;
00083         red_led = 0;
00084     }
00085 
00086 
00087 
00088 
00089     togglecolour = (togglecolour + 1) % 3;
00090 
00091 
00092 }
00093 
00094 ColourEnum Colour::getColour()
00095 {
00096     if (SNR > SNR_THRESHOLD_DB) {
00097         if ((colour >= -30*PI/180) && (colour < 30*PI/180)) {
00098             return BLUE;
00099         } else         if ((colour >= 30*PI/180) && (colour < 60*PI/180)) {
00100             return WHITE;
00101         } else         if ((colour >= 60*PI/180) && (colour < 120*PI/180)) {
00102             return RED;
00103         } else {
00104             return BLACK;
00105         }
00106     } else {
00107         return BLACK;
00108     }
00109 
00110 }