Diff: Beacon.cpp

Revision:

2:a9351d7f92b4

Parent:

0:d4427d048a98

--- a/Beacon.cpp	Fri Nov 02 03:00:29 2012 +0000
+++ b/Beacon.cpp	Sat Nov 03 01:03:16 2012 +0000
@@ -1,14 +1,137 @@
 #include "Beacon.h"
+#include "MODSERIAL.h"
+
+uint16_t
+Beacon::readAndAverageR(int times)
+{
+    int32_t avg = 0;
+    int i;
+    
+    for (i = 0; i < times; i++)
+    {
+        avg += ainR.read_u16();
+    }
+    
+    return (avg / times);
+}
+
+uint16_t
+Beacon::readAndAverageL(int times)
+{
+    int32_t avg = 0;
+    int i;
+    
+    for (i = 0; i < times; i++)
+    {
+        avg += ainL.read_u16();
+    }
+    
+    return (avg / times);
+}
+
+#define SAMPLES_TO_AVERAGE 8
+
+void
+Beacon::calibrate(MODSERIAL * debug)
+{
+    int32_t absMinL = 65536;
+    int32_t absMaxL = 0;
+    int32_t absMinR = 65536;
+    int32_t absMaxR = 0;
+    
+    uint16_t readingL;
+    uint16_t readingR;
+    
+    debug->printf("Calibrating offset and gain for beacon.  Turn off all beacons. Press any key when complete.\r\n");
+    while ( !debug->readable() )
+    {
+        wait(0.1);
+    }
+    debug->rxBufferFlush();
+    
+    debug->printf("Starting calibration.\r\n");
+    debug->printf("Absolute max and min starting values:\r\n");
+    debug->printf("\tminL = %d\r\n", absMinL);
+    debug->printf("\tmaxL = %d\r\n", absMaxL);
+    debug->printf("\tminR = %d\r\n", absMinR);
+    debug->printf("\tmaxR = %d\r\n", absMaxR);
+    
+    debug->printf("\r\nReading and averaging %d readings for left channel no beacon:\r\n", SAMPLES_TO_AVERAGE);
+    readingL = readAndAverageL(SAMPLES_TO_AVERAGE);
+    debug->printf("\tLeft channel average (beacon off): %5.5d\r\n", readingL);
+    
+    debug->printf("\r\nReading and averaging %d readings for right channel no beacon:\r\n", SAMPLES_TO_AVERAGE);
+    readingR = readAndAverageR(SAMPLES_TO_AVERAGE);
+    debug->printf("\tRight channel average (beacon off): %5.5d\r\n", readingR);
+    
+    absMinL = readingL;
+    absMinR = readingR;
+    
+    debug->printf("Minimums gathered:\r\n");
+    debug->printf("\tminL = %d\r\n", absMinL);
+    debug->printf("\tminR = %d\r\n", absMinR);
+    
+    debug->printf("Power on beacon and place directly in front and center of robot. Press any key to continue.\r\n");
+    while ( !debug->readable() )
+    {
+        debug->printf("rssiL = %d\r\n", readAndAverageL(SAMPLES_TO_AVERAGE));
+        debug->printf("rssiR = %d\r\n", readAndAverageR(SAMPLES_TO_AVERAGE));
+        wait(1);
+    }
+    debug->rxBufferFlush();
+    
+    
+    debug->printf("\r\nReading and averaging %d readings for left channel with beacon:\r\n", SAMPLES_TO_AVERAGE);
+    readingL = readAndAverageL(SAMPLES_TO_AVERAGE);
+    debug->printf("\tLeft channel average (beacon on): %5.5d\r\n", readingL);
+    
+    debug->printf("\r\nReading and averaging %d readings for right channel with beacon:\r\n", SAMPLES_TO_AVERAGE);
+    readingR = readAndAverageR(SAMPLES_TO_AVERAGE);
+    debug->printf("\tRight channel average (beacon on): %5.5d\r\n", readingR);
+    
+    absMaxL = readingL;
+    absMaxR = readingR;
+    
+    debug->printf("Maximums gathered:\r\n");
+    debug->printf("\tmaxL = %d\r\n", absMaxL);
+    debug->printf("\tmaxR = %d\r\n", absMaxR);
+    
+    debug->printf("Calculating gains:\r\n");
+    gainL = 65535 / (absMaxL - absMinL);
+    gainR = 65535 / (absMaxR - absMinR);
+    debug->printf("\tLeft  gain = %d\r\n", gainL);
+    debug->printf("\tRight gain = %d\r\n", gainR);
+    
+    debug->printf("Calculating offsets:\r\n");
+    offsL = absMinL;
+    offsR = absMinR;
+    debug->printf("\tLeft  offset = %d\r\n", offsL);
+    debug->printf("\tRight offset = %d\r\n", offsR);
+    
+    debug->printf("Calibration complete! Press any key to continue.\r\n");
+    while ( !debug->readable() );
+    debug->rxBufferFlush();
+}
 
 void
 Beacon::scan(void)
 {
-    valL = ainL.read_u16() >> 8;
-    valR = ainR.read_u16() >> 8;
+    int32_t tmpL = readAndAverageL(4) - offsL;
+    int32_t tmpR = readAndAverageR(4) - offsR;
+    
+    if (tmpL < 0) tmpL = 0;
+    if (tmpR < 0) tmpR = 0;
+    
+    valL = (tmpL * gainL) >> 8;
+    valR = (tmpR * gainR) >> 8;
+
+//    valL = ainL.read_u16() >> 8;
+//    valR = ainR.read_u16() >> 8;
+
 //    valR = ainL.read_u16() >> 8;
 //    valL = ainR.read_u16() >> 8;
     
-    valL > valR ?  max = valL : max = valR;
+    (valL > valR) ?  max = valL : max = valR;
     
     calc_centeroid();
 }
@@ -30,19 +153,19 @@
     return centeroid;
 }
 
-uint8_t
+uint16_t
 Beacon::get_max_rssi(void)
 {
     return max;
 }
 
-uint8_t
+uint16_t
 Beacon::getL(void)
 {
     return valL;
 }
 
-uint8_t
+uint16_t
 Beacon::getR(void)
 {
     return valR;