Cooper Liu / Mbed 2 deprecated ICRSEurobot13

Colour sensors calibrated

Dependencies:   mbed-rtos mbed Servo QEI

Fork of ICRSEurobot13 by Thomas Branch

Sensors/Colour/Colour.cpp@21:c592bf6a6a2d, 2013-04-12 (annotated)

Committer:
xiaxia686
Date:
Fri Apr 12 16:24:25 2013 +0000
Revision:
21:c592bf6a6a2d
Parent:
7:4340355261f9
Child:
45:77cf6375348a
Colour sensors calibrated

Who changed what in which revision?

UserRevisionLine numberNew contents of line
twighk 4:1be0f6c6ceae 1
twighk 4:1be0f6c6ceae 2 // Eurobot13 Colour.cpp
twighk 4:1be0f6c6ceae 3
twighk 4:1be0f6c6ceae 4 #include "Colour.h"
xiaxia686 21:c592bf6a6a2d 5 #include "mbed.h"
xiaxia686 21:c592bf6a6a2d 6 #include "math.h"
twighk 4:1be0f6c6ceae 7
xiaxia686 21:c592bf6a6a2d 8 Colour::Colour(PinName blue_led,
xiaxia686 21:c592bf6a6a2d 9 PinName red_led,
xiaxia686 21:c592bf6a6a2d 10 PinName pt,
xiaxia686 21:c592bf6a6a2d 11 ArmEnum arm)
xiaxia686 21:c592bf6a6a2d 12 : _blue_led(blue_led),
xiaxia686 21:c592bf6a6a2d 13 _red_led(red_led),
xiaxia686 21:c592bf6a6a2d 14 _pt(pt),
xiaxia686 21:c592bf6a6a2d 15 _arm(arm)
xiaxia686 21:c592bf6a6a2d 16 {
xiaxia686 21:c592bf6a6a2d 17
xiaxia686 21:c592bf6a6a2d 18
xiaxia686 21:c592bf6a6a2d 19 _ticker.attach(this, &Colour::_Blink, 0.01);
xiaxia686 21:c592bf6a6a2d 20
xiaxia686 21:c592bf6a6a2d 21 if (arm == UPPER){
xiaxia686 21:c592bf6a6a2d 22 red_correction_factor = UPPERARM_CORRECTION;
twighk 4:1be0f6c6ceae 23 }
xiaxia686 21:c592bf6a6a2d 24 else if (arm == LOWER){
xiaxia686 21:c592bf6a6a2d 25 red_correction_factor = LOWERARM_CORRECTION;
xiaxia686 21:c592bf6a6a2d 26 }
xiaxia686 21:c592bf6a6a2d 27 else {
xiaxia686 21:c592bf6a6a2d 28 red_correction_factor = 0.00f;
xiaxia686 21:c592bf6a6a2d 29 }
xiaxia686 21:c592bf6a6a2d 30
xiaxia686 21:c592bf6a6a2d 31
twighk 4:1be0f6c6ceae 32 }
twighk 4:1be0f6c6ceae 33
xiaxia686 21:c592bf6a6a2d 34 void Colour::_Blink (void)
xiaxia686 21:c592bf6a6a2d 35 {
xiaxia686 21:c592bf6a6a2d 36 static int toggle_colour = 0;
xiaxia686 21:c592bf6a6a2d 37 static float blue = 0;
xiaxia686 21:c592bf6a6a2d 38 static float blue_buff[BUFF_SIZE];
xiaxia686 21:c592bf6a6a2d 39 static float red = 0;
xiaxia686 21:c592bf6a6a2d 40 static float red_buff[BUFF_SIZE];
xiaxia686 21:c592bf6a6a2d 41 static float noise = 0;
xiaxia686 21:c592bf6a6a2d 42 static float noise_buff[BUFF_SIZE];
xiaxia686 21:c592bf6a6a2d 43
xiaxia686 21:c592bf6a6a2d 44 static int buff_pointer;
xiaxia686 21:c592bf6a6a2d 45
xiaxia686 21:c592bf6a6a2d 46 if (toggle_colour == 0) {
xiaxia686 21:c592bf6a6a2d 47
xiaxia686 21:c592bf6a6a2d 48 volatile float noise_temp = _pt.read();
xiaxia686 21:c592bf6a6a2d 49 noise += (noise_temp - noise_buff[buff_pointer])/BUFF_SIZE;
xiaxia686 21:c592bf6a6a2d 50 noise_buff[buff_pointer] = noise_temp;
xiaxia686 21:c592bf6a6a2d 51
xiaxia686 21:c592bf6a6a2d 52 buff_pointer = (buff_pointer + 1) % BUFF_SIZE;
xiaxia686 21:c592bf6a6a2d 53
xiaxia686 21:c592bf6a6a2d 54
xiaxia686 21:c592bf6a6a2d 55 _SNR = 20.0f*log10(hypot(blue,red)/noise);
xiaxia686 21:c592bf6a6a2d 56
xiaxia686 21:c592bf6a6a2d 57 volatile double blue_base = (blue - noise);
xiaxia686 21:c592bf6a6a2d 58 volatile double red_base = (red - noise)*red_correction_factor;
xiaxia686 21:c592bf6a6a2d 59 _colour = atan2(red_base,blue_base);
twighk 4:1be0f6c6ceae 60
xiaxia686 21:c592bf6a6a2d 61 //toggles leds for the next state
xiaxia686 21:c592bf6a6a2d 62 _blue_led = 1;
xiaxia686 21:c592bf6a6a2d 63 _red_led = 0;
xiaxia686 21:c592bf6a6a2d 64 } else if (toggle_colour == 1) {
xiaxia686 21:c592bf6a6a2d 65 volatile float blue_temp = _pt.read();
xiaxia686 21:c592bf6a6a2d 66 blue += (blue_temp - blue_buff[buff_pointer])/BUFF_SIZE;
xiaxia686 21:c592bf6a6a2d 67 blue_buff[buff_pointer] = blue_temp;
xiaxia686 21:c592bf6a6a2d 68 //toggles leds for the next state
xiaxia686 21:c592bf6a6a2d 69 _blue_led = 0;
xiaxia686 21:c592bf6a6a2d 70 _red_led = 1;
xiaxia686 21:c592bf6a6a2d 71 } else if (toggle_colour == 2) {
xiaxia686 21:c592bf6a6a2d 72 volatile float red_temp = _pt.read();
xiaxia686 21:c592bf6a6a2d 73 red += (red_temp - red_buff[buff_pointer])/BUFF_SIZE;
xiaxia686 21:c592bf6a6a2d 74 red_buff[buff_pointer] = red_temp;
xiaxia686 21:c592bf6a6a2d 75 //toggles leds for the next state
xiaxia686 21:c592bf6a6a2d 76 _blue_led = 0;
xiaxia686 21:c592bf6a6a2d 77 _red_led = 0;
xiaxia686 21:c592bf6a6a2d 78 }
xiaxia686 21:c592bf6a6a2d 79
xiaxia686 21:c592bf6a6a2d 80
xiaxia686 21:c592bf6a6a2d 81
xiaxia686 21:c592bf6a6a2d 82
xiaxia686 21:c592bf6a6a2d 83 toggle_colour = (toggle_colour + 1) % 3;
xiaxia686 21:c592bf6a6a2d 84
xiaxia686 21:c592bf6a6a2d 85
xiaxia686 21:c592bf6a6a2d 86 }
xiaxia686 21:c592bf6a6a2d 87
xiaxia686 21:c592bf6a6a2d 88 ColourEnum Colour::getColour()
xiaxia686 21:c592bf6a6a2d 89 {
xiaxia686 21:c592bf6a6a2d 90 if (_SNR > SNR_THRESHOLD_DB) {
xiaxia686 21:c592bf6a6a2d 91 if ((_colour >= -30*PI/180) && (_colour < 30*PI/180)) {
xiaxia686 21:c592bf6a6a2d 92 return BLUE;
xiaxia686 21:c592bf6a6a2d 93 } else if ((_colour >= 30*PI/180) && (_colour < 60*PI/180)) {
xiaxia686 21:c592bf6a6a2d 94 return WHITE;
xiaxia686 21:c592bf6a6a2d 95 } else if ((_colour >= 60*PI/180) && (_colour < 120*PI/180)) {
xiaxia686 21:c592bf6a6a2d 96 return RED;
xiaxia686 21:c592bf6a6a2d 97 } else {
xiaxia686 21:c592bf6a6a2d 98 return BLACK;
xiaxia686 21:c592bf6a6a2d 99 }
twighk 4:1be0f6c6ceae 100 } else {
xiaxia686 21:c592bf6a6a2d 101 return BLACK;
twighk 4:1be0f6c6ceae 102 }
xiaxia686 21:c592bf6a6a2d 103
xiaxia686 21:c592bf6a6a2d 104 }