Colour sensors calibrated
Dependencies: mbed-rtos mbed Servo QEI
Fork of ICRSEurobot13 by
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?
User | Revision | Line number | New 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 | } |