Mike Panetta / PiSlingers

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?

UserRevisionLine numberNew 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 }