File content as of revision 38:5e93a479c244:

#include "line_tracker.h"

LineTracker::LineTracker()
{
    for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
    {
        calibratedMaximum[s] = 0;   
        calibratedMinimum[s] = 0xFFFF;   
    }
    calibrationState = 0;
}

void LineTracker::read()
{
    readRawValues();
    updateCalibratedValues();
    updateLineStatus();
}

void LineTracker::readRawValues()
{
    readSensors(rawValues);
}

void LineTracker::updateCalibratedValues()
{
    for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
    {
        uint16_t calmin = calibratedMinimum[s];
        uint16_t calmax = calibratedMaximum[s];
        uint16_t denominator = calmax - calmin;
        int32_t x = 0;
        if(denominator != 0)
        {
            x = ((int32_t)rawValues[s] - calmin) * 1000 / denominator;
            if(x < 0)
            {
                x = 0;
            }
            else if(x > 1000)
            {
                x = 1000;
            }
        }
        calibratedValues[s] = x;
    }
}

void LineTracker::updateLineStatus()
{
    uint32_t avg = 0;
    uint32_t sum = 0;
    
    lineVisible = false;
    for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
    {
        // keep track of whether we see the line at all
        uint16_t value = calibratedValues[s];
        if (value > 500)
        {
            lineVisible = true;
        }
        
        // only average in values that are above a noise threshold
        if (value > 50)
        {
            avg += (uint32_t)(value) * s * 1000;
            sum += value;
        }
    }
    
    if (lineVisible)
    {
        linePosition = avg/sum;   
    }
    else
    {
        // We cannot see the line, so just snap the position to the left-most or right-most
        // depending on what we saw previousl.
        
        const uint32_t max = (LINE_SENSOR_COUNT-1)*1000;
        if(linePosition < max/2)
        {
            linePosition = 0;
        }
        else
        {
            linePosition = max;
        }
    }
}

// The return value of this should only be heeded if the calibration seems to be OK.
bool LineTracker::getLineVisible()
{
    return lineVisible;
}

uint16_t LineTracker::getLinePosition()
{
    return linePosition;   
}

void LineTracker::updateCalibration()
{
    if(calibrationState == 0)
    {
        for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
        {
            recentValuesMin[s] = 0xFFFF;
            recentValuesMax[s] = 0;
        }
    }

    for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
    {
        uint16_t value = rawValues[s];
        if (value < recentValuesMin[s]) { recentValuesMin[s] = value; }
        if (value > recentValuesMax[s]) { recentValuesMax[s] = value; }
    }

    calibrationState = calibrationState + 1;

    if (calibrationState == 9)
    {
        calibrationState = 0;   
        
        for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
        {
            if (recentValuesMin[s] > calibratedMaximum[s]) { calibratedMaximum[s] = recentValuesMin[s]; }
            if (recentValuesMax[s] < calibratedMinimum[s]) { calibratedMinimum[s] = recentValuesMax[s]; }
        }
    }    
}