Colour sensors calibrated
Dependencies: mbed-rtos mbed Servo QEI
Fork of ICRSEurobot13 by
Revision 45:77cf6375348a, committed 2013-04-12
- Comitter:
- xiaxia686
- Date:
- Fri Apr 12 20:40:52 2013 +0000
- Parent:
- 44:555136de33e4
- Child:
- 46:adcd57a5e402
- Commit message:
- colour sensors fixed
Changed in this revision
--- a/Sensors/Colour/Colour.cpp Fri Apr 12 16:46:42 2013 +0000 +++ b/Sensors/Colour/Colour.cpp Fri Apr 12 20:40:52 2013 +0000 @@ -3,96 +3,102 @@ #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) +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; + + + + + 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) +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) { + if (togglecolour == 0) { - volatile float noise_temp = _pt.read(); + 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); + 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); + 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 (toggle_colour == 1) { - volatile float blue_temp = _pt.read(); + 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; + 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(); + 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; + red_buff[buff_pointer] = red_temp; //toggles leds for the next state - _blue_led = 0; - _red_led = 0; + blue_led = 0; + red_led = 0; } - toggle_colour = (toggle_colour + 1) % 3; + togglecolour = (togglecolour + 1) % 3; } ColourEnum Colour::getColour() { - if (_SNR > SNR_THRESHOLD_DB) { - if ((_colour >= -30*PI/180) && (_colour < 30*PI/180)) { + 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)) { + } else if ((colour >= 30*PI/180) && (colour < 60*PI/180)) { return WHITE; - } else if ((_colour >= 60*PI/180) && (_colour < 120*PI/180)) { + } else if ((colour >= 60*PI/180) && (colour < 120*PI/180)) { return RED; } else { return BLACK;
--- a/Sensors/Colour/Colour.h Fri Apr 12 16:46:42 2013 +0000 +++ b/Sensors/Colour/Colour.h Fri Apr 12 20:40:52 2013 +0000 @@ -1,9 +1,11 @@ // Eurobot13 Colour.h - +#ifndef COLOUR_H +#define COLOUR_H #include "mbed.h" #include "globals.h" +#include "math.h" #define BUFF_SIZE 10 #define SNR_THRESHOLD_DB 4 @@ -17,25 +19,38 @@ class Colour{ public: + Colour( PinName blue_led, PinName red_led, PinName pt, ArmEnum arm); - ColourEnum getColour(); + virtual ColourEnum getColour(); private: - Ticker _ticker; - DigitalOut _blue_led; - DigitalOut _red_led; - AnalogIn _pt; - ArmEnum _arm; + Ticker ticker; + DigitalOut blue_led; + DigitalOut red_led; + AnalogIn pt; + ArmEnum arm; float red_correction_factor; - double _colour; - double _SNR; - void _Blink (void); + float colour; + float SNR; + void Blink(); -}; \ No newline at end of file + int togglecolour; + float blue; + float blue_buff[BUFF_SIZE]; + float red; + float red_buff[BUFF_SIZE]; + float noise; + float noise_buff[BUFF_SIZE]; + + int buff_pointer; + +}; + +#endif \ No newline at end of file
--- a/main.cpp Fri Apr 12 16:46:42 2013 +0000 +++ b/main.cpp Fri Apr 12 20:40:52 2013 +0000 @@ -76,28 +76,28 @@ pc.baud(115200); - InitSerial(); - wait(1); - Kalman::KalmanInit(); + // InitSerial(); + // wait(1); + // Kalman::KalmanInit(); - Thread predictthread(Kalman::predictloop, NULL, osPriorityNormal, 2084);//512); //temp 2k - Kalman::start_predict_ticker(&predictthread); + // Thread predictthread(Kalman::predictloop, NULL, osPriorityNormal, 2084);//512); //temp 2k + // Kalman::start_predict_ticker(&predictthread); - Thread updatethread(Kalman::updateloop, NULL, osPriorityNormal, 2084); + // Thread updatethread(Kalman::updateloop, NULL, osPriorityNormal, 2084); - Ticker motorcontroltestticker; - motorcontroltestticker.attach(MotorControl::motor_control_isr, 0.05); + // Ticker motorcontroltestticker; + // motorcontroltestticker.attach(MotorControl::motor_control_isr, 0.05); // ai layer thread - Thread aithread(AI::ailayer, NULL, osPriorityNormal, 2048); + // Thread aithread(AI::ailayer, NULL, osPriorityNormal, 2048); // motion layer periodic callback - RtosTimer motion_timer(motion::motionlayer, osTimerPeriodic); - motion_timer.start(50); + // RtosTimer motion_timer(motion::motionlayer, osTimerPeriodic); + // motion_timer.start(50); - Thread printingThread(Printing::printingloop, NULL, osPriorityLow, 2048); + // Thread printingThread(Printing::printingloop, NULL, osPriorityLow, 2048); - measureCPUidle(); //repurpose thread for idle measurement - //colourtest(); + // measureCPUidle(); //repurpose thread for idle measurement + colourtest(); //pt_test(); while(true) { Thread::wait(osWaitForever); @@ -174,14 +174,13 @@ void colourtest() { - //Colour c(P_COLOR_SENSOR_BLUE, P_COLOR_SENSOR_RED, P_COLOR_SENSOR_IN,UPPER); - Colour c(P_COLOR_SENSOR_BLUE_LOWER, P_COLOR_SENSOR_RED_LOWER, P_COLOR_SENSOR_IN_LOWER,LOWER); + Colour c_upper(P_COLOR_SENSOR_BLUE_UPPER, P_COLOR_SENSOR_RED_UPPER, P_COLOR_SENSOR_IN_UPPER,UPPER); + Colour c_lower(P_COLOR_SENSOR_BLUE_LOWER, P_COLOR_SENSOR_RED_LOWER, P_COLOR_SENSOR_IN_LOWER,LOWER); while(true) { wait(0.1); - ColourEnum ce = c.getColour(); - switch(ce) { + switch(c_lower.getColour()) { case BLUE : printf("BLUE\n"); break; @@ -206,7 +205,7 @@ { DigitalOut _blue_led(p10); DigitalOut _red_led(p11); - AnalogIn _pt(p19); + AnalogIn _pt(p18); bytepack_t datapackage; // first 3 bytes of header is used for alignment