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