line_tracker.cpp

00001 #include "line_tracker.h"
00002 
00003 LineTracker::LineTracker()
00004 {
00005     for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
00006     {
00007         calibratedMaximum[s] = 0;   
00008         calibratedMinimum[s] = 0xFFFF;   
00009     }
00010     calibrationState = 0;
00011 }
00012 
00013 void LineTracker::read()
00014 {
00015     readRawValues();
00016     updateCalibratedValues();
00017     updateLineStatus();
00018 }
00019 
00020 void LineTracker::readRawValues()
00021 {
00022     readSensors(rawValues);
00023 }
00024 
00025 void LineTracker::updateCalibratedValues()
00026 {
00027     for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
00028     {
00029         uint16_t calmin = calibratedMinimum[s];
00030         uint16_t calmax = calibratedMaximum[s];
00031         uint16_t denominator = calmax - calmin;
00032         int32_t x = 0;
00033         if(denominator != 0)
00034         {
00035             x = ((int32_t)rawValues[s] - calmin) * 1000 / denominator;
00036             if(x < 0)
00037             {
00038                 x = 0;
00039             }
00040             else if(x > 1000)
00041             {
00042                 x = 1000;
00043             }
00044         }
00045         calibratedValues[s] = x;
00046     }
00047 }
00048 
00049 void LineTracker::updateLineStatus()
00050 {
00051     uint32_t avg = 0;
00052     uint32_t sum = 0;
00053     
00054     lineVisible = false;
00055     for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
00056     {
00057         // keep track of whether we see the line at all
00058         uint16_t value = calibratedValues[s];
00059         if (value > 500)
00060         {
00061             lineVisible = true;
00062         }
00063         
00064         // only average in values that are above a noise threshold
00065         if (value > 50)
00066         {
00067             avg += (uint32_t)(value) * s * 1000;
00068             sum += value;
00069         }
00070     }
00071     
00072     if (lineVisible)
00073     {
00074         linePosition = avg/sum;   
00075     }
00076     else
00077     {
00078         // We cannot see the line, so just snap the position to the left-most or right-most
00079         // depending on what we saw previousl.
00080         
00081         const uint32_t max = (LINE_SENSOR_COUNT-1)*1000;
00082         if(linePosition < max/2)
00083         {
00084             linePosition = 0;
00085         }
00086         else
00087         {
00088             linePosition = max;
00089         }
00090     }
00091 }
00092 
00093 // The return value of this should only be heeded if the calibration seems to be OK.
00094 bool LineTracker::getLineVisible()
00095 {
00096     return lineVisible;
00097 }
00098 
00099 uint16_t LineTracker::getLinePosition()
00100 {
00101     return linePosition;   
00102 }
00103 
00104 void LineTracker::updateCalibration()
00105 {
00106     if(calibrationState == 0)
00107     {
00108         for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
00109         {
00110             recentValuesMin[s] = 0xFFFF;
00111             recentValuesMax[s] = 0;
00112         }
00113     }
00114 
00115     for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
00116     {
00117         uint16_t value = rawValues[s];
00118         if (value < recentValuesMin[s]) { recentValuesMin[s] = value; }
00119         if (value > recentValuesMax[s]) { recentValuesMax[s] = value; }
00120     }
00121 
00122     calibrationState = calibrationState + 1;
00123 
00124     if (calibrationState == 9)
00125     {
00126         calibrationState = 0;   
00127         
00128         for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
00129         {
00130             if (recentValuesMin[s] > calibratedMaximum[s]) { calibratedMaximum[s] = recentValuesMin[s]; }
00131             if (recentValuesMax[s] < calibratedMinimum[s]) { calibratedMinimum[s] = recentValuesMax[s]; }
00132         }
00133     }    
00134 }