Diff: line_tracker.cpp

Revision:

21:c279c6a83671

Child:

22:44c032e59ff5

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/line_tracker.cpp	Thu Feb 27 23:20:34 2014 +0000
@@ -0,0 +1,133 @@
+#include "line_tracker.h"
+
+static uint16_t readSensor(uint8_t index)
+{
+    return lineSensorsAnalog[index].read_u16();
+}
+
+LineTracker::LineTracker()
+{
+    for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
+    {
+        calibratedMaximum[s] = 0;   
+        calibratedMinimum[s] = 0xFFFF;   
+    }
+}
+
+void LineTracker::updateCalibration()
+{
+    const int sampleCount = 10;
+
+    uint16_t maxValues[LINE_SENSOR_COUNT];
+    uint16_t minValues[LINE_SENSOR_COUNT];
+    
+    for(uint8_t sample = 0; sample < sampleCount; sample++)
+    {
+        for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
+        {
+            uint16_t reading = readSensor(s);
+            if (reading > maxValues[s]) { maxValues[s] = reading; }
+            if (reading < minValues[s]) { minValues[s] = reading; }
+        }
+    }
+    
+    for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
+    {
+        if (minValues[s] > calibratedMaximum[s]) { calibratedMaximum[s] = minValues[s]; }
+        if (maxValues[s] > calibratedMinimum[s]) { calibratedMinimum[s] = maxValues[s]; }
+    }
+}
+
+void LineTracker::read()
+{
+    readRawValues();
+    updateCalibratedValues();
+    updateLineStatus();
+}
+
+void LineTracker::readRawValues()
+{
+    for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
+    {
+        rawValues[s] = readSensor(s);
+    }
+}
+
+void LineTracker::updateCalibratedValues()
+{
+    // Update the calibrated values.
+    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;
+        int16_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;
+    
+    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 > 200)
+        {
+            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;   
+}
\ No newline at end of file