PiSlingers library for AHRC competitions
Beacon.cpp@2:a9351d7f92b4, 2012-11-03 (annotated)
- Committer:
- mpanetta
- Date:
- Sat Nov 03 01:03:16 2012 +0000
- Revision:
- 2:a9351d7f92b4
- Parent:
- 0:d4427d048a98
Fixed some bugs in the IR code that allowed negative values through.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
mpanetta | 0:d4427d048a98 | 1 | #include "Beacon.h" |
mpanetta | 2:a9351d7f92b4 | 2 | #include "MODSERIAL.h" |
mpanetta | 2:a9351d7f92b4 | 3 | |
mpanetta | 2:a9351d7f92b4 | 4 | uint16_t |
mpanetta | 2:a9351d7f92b4 | 5 | Beacon::readAndAverageR(int times) |
mpanetta | 2:a9351d7f92b4 | 6 | { |
mpanetta | 2:a9351d7f92b4 | 7 | int32_t avg = 0; |
mpanetta | 2:a9351d7f92b4 | 8 | int i; |
mpanetta | 2:a9351d7f92b4 | 9 | |
mpanetta | 2:a9351d7f92b4 | 10 | for (i = 0; i < times; i++) |
mpanetta | 2:a9351d7f92b4 | 11 | { |
mpanetta | 2:a9351d7f92b4 | 12 | avg += ainR.read_u16(); |
mpanetta | 2:a9351d7f92b4 | 13 | } |
mpanetta | 2:a9351d7f92b4 | 14 | |
mpanetta | 2:a9351d7f92b4 | 15 | return (avg / times); |
mpanetta | 2:a9351d7f92b4 | 16 | } |
mpanetta | 2:a9351d7f92b4 | 17 | |
mpanetta | 2:a9351d7f92b4 | 18 | uint16_t |
mpanetta | 2:a9351d7f92b4 | 19 | Beacon::readAndAverageL(int times) |
mpanetta | 2:a9351d7f92b4 | 20 | { |
mpanetta | 2:a9351d7f92b4 | 21 | int32_t avg = 0; |
mpanetta | 2:a9351d7f92b4 | 22 | int i; |
mpanetta | 2:a9351d7f92b4 | 23 | |
mpanetta | 2:a9351d7f92b4 | 24 | for (i = 0; i < times; i++) |
mpanetta | 2:a9351d7f92b4 | 25 | { |
mpanetta | 2:a9351d7f92b4 | 26 | avg += ainL.read_u16(); |
mpanetta | 2:a9351d7f92b4 | 27 | } |
mpanetta | 2:a9351d7f92b4 | 28 | |
mpanetta | 2:a9351d7f92b4 | 29 | return (avg / times); |
mpanetta | 2:a9351d7f92b4 | 30 | } |
mpanetta | 2:a9351d7f92b4 | 31 | |
mpanetta | 2:a9351d7f92b4 | 32 | #define SAMPLES_TO_AVERAGE 8 |
mpanetta | 2:a9351d7f92b4 | 33 | |
mpanetta | 2:a9351d7f92b4 | 34 | void |
mpanetta | 2:a9351d7f92b4 | 35 | Beacon::calibrate(MODSERIAL * debug) |
mpanetta | 2:a9351d7f92b4 | 36 | { |
mpanetta | 2:a9351d7f92b4 | 37 | int32_t absMinL = 65536; |
mpanetta | 2:a9351d7f92b4 | 38 | int32_t absMaxL = 0; |
mpanetta | 2:a9351d7f92b4 | 39 | int32_t absMinR = 65536; |
mpanetta | 2:a9351d7f92b4 | 40 | int32_t absMaxR = 0; |
mpanetta | 2:a9351d7f92b4 | 41 | |
mpanetta | 2:a9351d7f92b4 | 42 | uint16_t readingL; |
mpanetta | 2:a9351d7f92b4 | 43 | uint16_t readingR; |
mpanetta | 2:a9351d7f92b4 | 44 | |
mpanetta | 2:a9351d7f92b4 | 45 | debug->printf("Calibrating offset and gain for beacon. Turn off all beacons. Press any key when complete.\r\n"); |
mpanetta | 2:a9351d7f92b4 | 46 | while ( !debug->readable() ) |
mpanetta | 2:a9351d7f92b4 | 47 | { |
mpanetta | 2:a9351d7f92b4 | 48 | wait(0.1); |
mpanetta | 2:a9351d7f92b4 | 49 | } |
mpanetta | 2:a9351d7f92b4 | 50 | debug->rxBufferFlush(); |
mpanetta | 2:a9351d7f92b4 | 51 | |
mpanetta | 2:a9351d7f92b4 | 52 | debug->printf("Starting calibration.\r\n"); |
mpanetta | 2:a9351d7f92b4 | 53 | debug->printf("Absolute max and min starting values:\r\n"); |
mpanetta | 2:a9351d7f92b4 | 54 | debug->printf("\tminL = %d\r\n", absMinL); |
mpanetta | 2:a9351d7f92b4 | 55 | debug->printf("\tmaxL = %d\r\n", absMaxL); |
mpanetta | 2:a9351d7f92b4 | 56 | debug->printf("\tminR = %d\r\n", absMinR); |
mpanetta | 2:a9351d7f92b4 | 57 | debug->printf("\tmaxR = %d\r\n", absMaxR); |
mpanetta | 2:a9351d7f92b4 | 58 | |
mpanetta | 2:a9351d7f92b4 | 59 | debug->printf("\r\nReading and averaging %d readings for left channel no beacon:\r\n", SAMPLES_TO_AVERAGE); |
mpanetta | 2:a9351d7f92b4 | 60 | readingL = readAndAverageL(SAMPLES_TO_AVERAGE); |
mpanetta | 2:a9351d7f92b4 | 61 | debug->printf("\tLeft channel average (beacon off): %5.5d\r\n", readingL); |
mpanetta | 2:a9351d7f92b4 | 62 | |
mpanetta | 2:a9351d7f92b4 | 63 | debug->printf("\r\nReading and averaging %d readings for right channel no beacon:\r\n", SAMPLES_TO_AVERAGE); |
mpanetta | 2:a9351d7f92b4 | 64 | readingR = readAndAverageR(SAMPLES_TO_AVERAGE); |
mpanetta | 2:a9351d7f92b4 | 65 | debug->printf("\tRight channel average (beacon off): %5.5d\r\n", readingR); |
mpanetta | 2:a9351d7f92b4 | 66 | |
mpanetta | 2:a9351d7f92b4 | 67 | absMinL = readingL; |
mpanetta | 2:a9351d7f92b4 | 68 | absMinR = readingR; |
mpanetta | 2:a9351d7f92b4 | 69 | |
mpanetta | 2:a9351d7f92b4 | 70 | debug->printf("Minimums gathered:\r\n"); |
mpanetta | 2:a9351d7f92b4 | 71 | debug->printf("\tminL = %d\r\n", absMinL); |
mpanetta | 2:a9351d7f92b4 | 72 | debug->printf("\tminR = %d\r\n", absMinR); |
mpanetta | 2:a9351d7f92b4 | 73 | |
mpanetta | 2:a9351d7f92b4 | 74 | debug->printf("Power on beacon and place directly in front and center of robot. Press any key to continue.\r\n"); |
mpanetta | 2:a9351d7f92b4 | 75 | while ( !debug->readable() ) |
mpanetta | 2:a9351d7f92b4 | 76 | { |
mpanetta | 2:a9351d7f92b4 | 77 | debug->printf("rssiL = %d\r\n", readAndAverageL(SAMPLES_TO_AVERAGE)); |
mpanetta | 2:a9351d7f92b4 | 78 | debug->printf("rssiR = %d\r\n", readAndAverageR(SAMPLES_TO_AVERAGE)); |
mpanetta | 2:a9351d7f92b4 | 79 | wait(1); |
mpanetta | 2:a9351d7f92b4 | 80 | } |
mpanetta | 2:a9351d7f92b4 | 81 | debug->rxBufferFlush(); |
mpanetta | 2:a9351d7f92b4 | 82 | |
mpanetta | 2:a9351d7f92b4 | 83 | |
mpanetta | 2:a9351d7f92b4 | 84 | debug->printf("\r\nReading and averaging %d readings for left channel with beacon:\r\n", SAMPLES_TO_AVERAGE); |
mpanetta | 2:a9351d7f92b4 | 85 | readingL = readAndAverageL(SAMPLES_TO_AVERAGE); |
mpanetta | 2:a9351d7f92b4 | 86 | debug->printf("\tLeft channel average (beacon on): %5.5d\r\n", readingL); |
mpanetta | 2:a9351d7f92b4 | 87 | |
mpanetta | 2:a9351d7f92b4 | 88 | debug->printf("\r\nReading and averaging %d readings for right channel with beacon:\r\n", SAMPLES_TO_AVERAGE); |
mpanetta | 2:a9351d7f92b4 | 89 | readingR = readAndAverageR(SAMPLES_TO_AVERAGE); |
mpanetta | 2:a9351d7f92b4 | 90 | debug->printf("\tRight channel average (beacon on): %5.5d\r\n", readingR); |
mpanetta | 2:a9351d7f92b4 | 91 | |
mpanetta | 2:a9351d7f92b4 | 92 | absMaxL = readingL; |
mpanetta | 2:a9351d7f92b4 | 93 | absMaxR = readingR; |
mpanetta | 2:a9351d7f92b4 | 94 | |
mpanetta | 2:a9351d7f92b4 | 95 | debug->printf("Maximums gathered:\r\n"); |
mpanetta | 2:a9351d7f92b4 | 96 | debug->printf("\tmaxL = %d\r\n", absMaxL); |
mpanetta | 2:a9351d7f92b4 | 97 | debug->printf("\tmaxR = %d\r\n", absMaxR); |
mpanetta | 2:a9351d7f92b4 | 98 | |
mpanetta | 2:a9351d7f92b4 | 99 | debug->printf("Calculating gains:\r\n"); |
mpanetta | 2:a9351d7f92b4 | 100 | gainL = 65535 / (absMaxL - absMinL); |
mpanetta | 2:a9351d7f92b4 | 101 | gainR = 65535 / (absMaxR - absMinR); |
mpanetta | 2:a9351d7f92b4 | 102 | debug->printf("\tLeft gain = %d\r\n", gainL); |
mpanetta | 2:a9351d7f92b4 | 103 | debug->printf("\tRight gain = %d\r\n", gainR); |
mpanetta | 2:a9351d7f92b4 | 104 | |
mpanetta | 2:a9351d7f92b4 | 105 | debug->printf("Calculating offsets:\r\n"); |
mpanetta | 2:a9351d7f92b4 | 106 | offsL = absMinL; |
mpanetta | 2:a9351d7f92b4 | 107 | offsR = absMinR; |
mpanetta | 2:a9351d7f92b4 | 108 | debug->printf("\tLeft offset = %d\r\n", offsL); |
mpanetta | 2:a9351d7f92b4 | 109 | debug->printf("\tRight offset = %d\r\n", offsR); |
mpanetta | 2:a9351d7f92b4 | 110 | |
mpanetta | 2:a9351d7f92b4 | 111 | debug->printf("Calibration complete! Press any key to continue.\r\n"); |
mpanetta | 2:a9351d7f92b4 | 112 | while ( !debug->readable() ); |
mpanetta | 2:a9351d7f92b4 | 113 | debug->rxBufferFlush(); |
mpanetta | 2:a9351d7f92b4 | 114 | } |
mpanetta | 0:d4427d048a98 | 115 | |
mpanetta | 0:d4427d048a98 | 116 | void |
mpanetta | 0:d4427d048a98 | 117 | Beacon::scan(void) |
mpanetta | 0:d4427d048a98 | 118 | { |
mpanetta | 2:a9351d7f92b4 | 119 | int32_t tmpL = readAndAverageL(4) - offsL; |
mpanetta | 2:a9351d7f92b4 | 120 | int32_t tmpR = readAndAverageR(4) - offsR; |
mpanetta | 2:a9351d7f92b4 | 121 | |
mpanetta | 2:a9351d7f92b4 | 122 | if (tmpL < 0) tmpL = 0; |
mpanetta | 2:a9351d7f92b4 | 123 | if (tmpR < 0) tmpR = 0; |
mpanetta | 2:a9351d7f92b4 | 124 | |
mpanetta | 2:a9351d7f92b4 | 125 | valL = (tmpL * gainL) >> 8; |
mpanetta | 2:a9351d7f92b4 | 126 | valR = (tmpR * gainR) >> 8; |
mpanetta | 2:a9351d7f92b4 | 127 | |
mpanetta | 2:a9351d7f92b4 | 128 | // valL = ainL.read_u16() >> 8; |
mpanetta | 2:a9351d7f92b4 | 129 | // valR = ainR.read_u16() >> 8; |
mpanetta | 2:a9351d7f92b4 | 130 | |
mpanetta | 0:d4427d048a98 | 131 | // valR = ainL.read_u16() >> 8; |
mpanetta | 0:d4427d048a98 | 132 | // valL = ainR.read_u16() >> 8; |
mpanetta | 0:d4427d048a98 | 133 | |
mpanetta | 2:a9351d7f92b4 | 134 | (valL > valR) ? max = valL : max = valR; |
mpanetta | 0:d4427d048a98 | 135 | |
mpanetta | 0:d4427d048a98 | 136 | calc_centeroid(); |
mpanetta | 0:d4427d048a98 | 137 | } |
mpanetta | 0:d4427d048a98 | 138 | |
mpanetta | 0:d4427d048a98 | 139 | void |
mpanetta | 0:d4427d048a98 | 140 | Beacon::calc_centeroid(void) |
mpanetta | 0:d4427d048a98 | 141 | { |
mpanetta | 0:d4427d048a98 | 142 | float sumA, sumB; |
mpanetta | 0:d4427d048a98 | 143 | |
mpanetta | 0:d4427d048a98 | 144 | sumB = (valL * 1) + (valR * 2); |
mpanetta | 0:d4427d048a98 | 145 | sumA = valL + valR; |
mpanetta | 0:d4427d048a98 | 146 | |
mpanetta | 0:d4427d048a98 | 147 | centeroid = sumB / sumA - 1.5f; |
mpanetta | 0:d4427d048a98 | 148 | } |
mpanetta | 0:d4427d048a98 | 149 | |
mpanetta | 0:d4427d048a98 | 150 | float |
mpanetta | 0:d4427d048a98 | 151 | Beacon::get_centeroid(void) |
mpanetta | 0:d4427d048a98 | 152 | { |
mpanetta | 0:d4427d048a98 | 153 | return centeroid; |
mpanetta | 0:d4427d048a98 | 154 | } |
mpanetta | 0:d4427d048a98 | 155 | |
mpanetta | 2:a9351d7f92b4 | 156 | uint16_t |
mpanetta | 0:d4427d048a98 | 157 | Beacon::get_max_rssi(void) |
mpanetta | 0:d4427d048a98 | 158 | { |
mpanetta | 0:d4427d048a98 | 159 | return max; |
mpanetta | 0:d4427d048a98 | 160 | } |
mpanetta | 0:d4427d048a98 | 161 | |
mpanetta | 2:a9351d7f92b4 | 162 | uint16_t |
mpanetta | 0:d4427d048a98 | 163 | Beacon::getL(void) |
mpanetta | 0:d4427d048a98 | 164 | { |
mpanetta | 0:d4427d048a98 | 165 | return valL; |
mpanetta | 0:d4427d048a98 | 166 | } |
mpanetta | 0:d4427d048a98 | 167 | |
mpanetta | 2:a9351d7f92b4 | 168 | uint16_t |
mpanetta | 0:d4427d048a98 | 169 | Beacon::getR(void) |
mpanetta | 0:d4427d048a98 | 170 | { |
mpanetta | 0:d4427d048a98 | 171 | return valR; |
mpanetta | 0:d4427d048a98 | 172 | } |