This is some awesome robot code
Dependencies: mbed-rtos mbed QEI
Fork of ICRSEurobot13 by
Sensors/Colour/Colour.cpp@90:e4164bb8c60e, 2013-04-17 (annotated)
- Committer:
- madcowswe
- Date:
- Wed Apr 17 23:16:25 2013 +0000
- Revision:
- 90:e4164bb8c60e
- Parent:
- 45:77cf6375348a
final state at end of competition. Includes avoid wooden team hack
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 | 43: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 | 43:c592bf6a6a2d | 16 | { |
xiaxia686 | 43:c592bf6a6a2d | 17 | |
xiaxia686 | 43: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 | 43:c592bf6a6a2d | 29 | } |
xiaxia686 | 43: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 | 43:c592bf6a6a2d | 45 | |
twighk | 4:1be0f6c6ceae | 46 | } |
twighk | 4:1be0f6c6ceae | 47 | |
xiaxia686 | 45:77cf6375348a | 48 | void Colour::Blink (void) |
xiaxia686 | 43:c592bf6a6a2d | 49 | { |
xiaxia686 | 45:77cf6375348a | 50 | |
xiaxia686 | 43:c592bf6a6a2d | 51 | |
xiaxia686 | 45:77cf6375348a | 52 | if (togglecolour == 0) { |
xiaxia686 | 43:c592bf6a6a2d | 53 | |
xiaxia686 | 45:77cf6375348a | 54 | float noise_temp = pt.read(); |
xiaxia686 | 43:c592bf6a6a2d | 55 | noise += (noise_temp - noise_buff[buff_pointer])/BUFF_SIZE; |
xiaxia686 | 43:c592bf6a6a2d | 56 | noise_buff[buff_pointer] = noise_temp; |
xiaxia686 | 45:77cf6375348a | 57 | |
xiaxia686 | 43:c592bf6a6a2d | 58 | buff_pointer = (buff_pointer + 1) % BUFF_SIZE; |
xiaxia686 | 43:c592bf6a6a2d | 59 | |
xiaxia686 | 43:c592bf6a6a2d | 60 | |
xiaxia686 | 45:77cf6375348a | 61 | SNR = 20.0f*log10(hypot(blue,red)/noise); |
xiaxia686 | 43: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 | 43: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 | 43:c592bf6a6a2d | 72 | blue += (blue_temp - blue_buff[buff_pointer])/BUFF_SIZE; |
xiaxia686 | 45:77cf6375348a | 73 | blue_buff[buff_pointer] = blue_temp; |
xiaxia686 | 43: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 | 43:c592bf6a6a2d | 79 | red += (red_temp - red_buff[buff_pointer])/BUFF_SIZE; |
xiaxia686 | 45:77cf6375348a | 80 | red_buff[buff_pointer] = red_temp; |
xiaxia686 | 43:c592bf6a6a2d | 81 | //toggles leds for the next state |
xiaxia686 | 45:77cf6375348a | 82 | blue_led = 0; |
xiaxia686 | 45:77cf6375348a | 83 | red_led = 0; |
xiaxia686 | 43:c592bf6a6a2d | 84 | } |
xiaxia686 | 43:c592bf6a6a2d | 85 | |
xiaxia686 | 43:c592bf6a6a2d | 86 | |
xiaxia686 | 43:c592bf6a6a2d | 87 | |
xiaxia686 | 43:c592bf6a6a2d | 88 | |
xiaxia686 | 45:77cf6375348a | 89 | togglecolour = (togglecolour + 1) % 3; |
xiaxia686 | 43:c592bf6a6a2d | 90 | |
xiaxia686 | 43:c592bf6a6a2d | 91 | |
xiaxia686 | 43:c592bf6a6a2d | 92 | } |
xiaxia686 | 43:c592bf6a6a2d | 93 | |
xiaxia686 | 43:c592bf6a6a2d | 94 | ColourEnum Colour::getColour() |
xiaxia686 | 43: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 | 43:c592bf6a6a2d | 98 | return BLUE; |
xiaxia686 | 45:77cf6375348a | 99 | } else if ((colour >= 30*PI/180) && (colour < 60*PI/180)) { |
xiaxia686 | 43:c592bf6a6a2d | 100 | return WHITE; |
xiaxia686 | 45:77cf6375348a | 101 | } else if ((colour >= 60*PI/180) && (colour < 120*PI/180)) { |
xiaxia686 | 43:c592bf6a6a2d | 102 | return RED; |
xiaxia686 | 43:c592bf6a6a2d | 103 | } else { |
xiaxia686 | 43:c592bf6a6a2d | 104 | return BLACK; |
xiaxia686 | 43:c592bf6a6a2d | 105 | } |
twighk | 4:1be0f6c6ceae | 106 | } else { |
xiaxia686 | 43:c592bf6a6a2d | 107 | return BLACK; |
twighk | 4:1be0f6c6ceae | 108 | } |
xiaxia686 | 43:c592bf6a6a2d | 109 | |
xiaxia686 | 43:c592bf6a6a2d | 110 | } |