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@45:77cf6375348a, 2013-04-12 (annotated)

Committer:
xiaxia686
Date:
Fri Apr 12 20:40:52 2013 +0000
Revision:
45:77cf6375348a
Parent:
21:c592bf6a6a2d 
colour sensors fixed

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