Colour sensors calibrated
Dependencies: mbed-rtos mbed Servo QEI
Fork of ICRSEurobot13 by
Sensors/Colour/Colour.cpp
- Committer:
- xiaxia686
- Date:
- 2013-04-12
- Revision:
- 45:77cf6375348a
- Parent:
- 21:c592bf6a6a2d
File content as of revision 45:77cf6375348a:
// 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; } }