File content as of revision 2:a9351d7f92b4:

#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)
{
    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;
    
    calc_centeroid();
}

void
Beacon::calc_centeroid(void)
{
    float sumA, sumB;
    
    sumB = (valL * 1) + (valR * 2);
    sumA = valL + valR;
    
    centeroid = sumB / sumA - 1.5f;
}

float
Beacon::get_centeroid(void)
{
    return centeroid;
}

uint16_t
Beacon::get_max_rssi(void)
{
    return max;
}

uint16_t
Beacon::getL(void)
{
    return valL;
}

uint16_t
Beacon::getR(void)
{
    return valR;
}