Revision 2:a9351d7f92b4, committed 2012-11-03

Comitter:

mpanetta

Date:

Sat Nov 03 01:03:16 2012 +0000

Parent:

1:695f4f4442d3

Commit message:

Fixed some bugs in the IR code that allowed negative values through.

Changed in this revision

diff -r 695f4f4442d3 -r a9351d7f92b4 Beacon.cpp
--- 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;

diff -r 695f4f4442d3 -r a9351d7f92b4 Beacon.h
--- a/Beacon.h	Fri Nov 02 03:00:29 2012 +0000
+++ b/Beacon.h	Sat Nov 03 01:03:16 2012 +0000
@@ -2,29 +2,45 @@
 #define _BEACON_H_
 
 #include "mbed.h"
+#include "MODSERIAL.h"
 
 class Beacon 
 {
 public:
-    Beacon(void) : ainL(p16), ainR(p17) {};
+    Beacon(void) : ainL(p16), ainR(p17) 
+    {
+        offsL = 0;
+        gainL = 1;
+        offsR = 0;
+        gainR = 1;
+    };
     
     void    scan(void);
-    uint8_t get_max_rssi(void);
+    void    calibrate(MODSERIAL * debug);
+    uint16_t get_max_rssi(void);
     float   get_centeroid(void);
     
-    uint8_t getL(void);
-    uint8_t getR(void);
+    uint16_t getL(void);
+    uint16_t getR(void);
     
 private:
+    uint16_t readAndAverageL(int times);
+    uint16_t readAndAverageR(int times);
+    
     void calc_centeroid(void);
 
     AnalogIn ainL;
     AnalogIn ainR;
     
-    uint8_t  valL;
-    uint8_t  valR;
+    uint16_t  valL;
+    uint16_t  valR;
     
-    uint8_t  max;
+    int32_t   offsL;
+    int32_t   gainL;
+    int32_t   offsR;
+    int32_t   gainR;
+    
+    uint16_t  max;
     float    centeroid;
 
 };

diff -r 695f4f4442d3 -r a9351d7f92b4 IRObjDetector.cpp
--- a/IRObjDetector.cpp	Fri Nov 02 03:00:29 2012 +0000
+++ b/IRObjDetector.cpp	Sat Nov 03 01:03:16 2012 +0000
@@ -42,10 +42,13 @@
         tmp += ain.read_u16();
         
         // Take reading and subtract off ambient.
-        tmp = tmp / 4 - data[7];
+        tmp = (tmp / 4) - data[7];
         
-        if (tmp < 0) 
+        if (tmp < 0)
+        { 
             data[i] = 0;
+            tmp     = 0;
+        }
         else
             data[i] = tmp >> 8;