Diff: Vision.cpp

Revision:

0:ccbc44580fab

Child:

1:68bb92736e14

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Vision.cpp	Fri Mar 25 13:05:09 2016 +0000
@@ -0,0 +1,128 @@
+#include "TFC.h"
+#include "Vision.h"
+
+const uint16_t _vision_lightcomp[] = {403,372,303,219,160,111,85,70,60,53,47,43,41,37,36,33,32,31,29,29,28,27,27,26,26,25,25,25,24,24,24,23,23,23,23,22,22,22,21,22,21,21,21,21,21,21,21,21,21,20,21,21,20,21,21,20,20,20,21,21,21,20,20,21,20,20,20,20,20,20,20,20,20,21,20,21,20,20,20,20,20,20,21,21,20,20,20,21,20,21,21,21,20,21,21,22,22,22,22,22,23,23,23,23,23,25,24,25,25,26,26,27,27,29,29,31,32,35,38,41,43,46,47,53,62,77,93,113};
+
+Vision::Vision()
+{
+    lightCompensation = false;
+    newDataToProcess = false;
+}
+
+void Vision::saveData(uint16_t* newData)
+{
+    uint8_t i;
+    
+    for(i=0;i<128;i++)
+    {
+        lineData[i] = newData[i];
+    }
+    newDataToProcess = true;
+}
+
+struct lineScanData Vision::processLine()
+{
+    uint32_t edgeProcessingBuffer[128];
+    int16_t edgeGradients[128];
+    uint8_t i;
+    uint32_t maxWhite = 0;
+    uint8_t maxWhiteIndex = 64;
+    uint16_t edgeThreshold = 100;
+    struct lineScanData result;
+    
+//    uint32_t maxWhiteSum = 0;
+//    uint8_t whiteAreaDelta = 5;
+    
+    // remove noise from data
+    for(i=2;i<126;i++)
+    {
+        // apply gaussian filter
+        edgeProcessingBuffer[i] = (lineData[i-2]+lineData[i+2])*5+(lineData[i-1]+lineData[i+1])*12+lineData[i]*15; // filtered data is multiplied by 49 here
+        if(edgeProcessingBuffer[i] > maxWhite)
+        {
+            maxWhite = edgeProcessingBuffer[i];
+            maxWhiteIndex = i;
+        }
+    }
+    
+/*    // Find the area of 2*whiteAreaDelta which is the brighter
+    // init max white search
+    maxWhiteSum = 0;
+    for(i=2;i<(2+whiteAreaDelta*2);i++)
+    {
+        maxWhiteSum += edgeProcessingBuffer[i];
+    }
+    maxWhite = maxWhiteSum;
+    maxWhiteIndex = 2+whiteAreaDelta;
+    for(i=3+whiteAreaDelta;i<126-whiteAreaDelta;i++)
+    {
+        maxWhiteSum += edgeProcessingBuffer[i+whiteAreaDelta-1]-edgeProcessingBuffer[i-whiteAreaDelta-1];
+        if(maxWhiteSum > maxWhite)
+        {
+            maxWhite = maxWhiteSum;
+            maxWhiteIndex = i;
+        }
+    }*/
+    
+    result.mostWhiteIndex = maxWhiteIndex;
+    result.maxLightValue = (uint16_t)(maxWhite/49);
+    edgeThreshold = maxWhite/20;
+    
+    // find gradients
+    for(i=2;i<125;i++)
+    {
+        edgeGradients[i] = edgeProcessingBuffer[i]-edgeProcessingBuffer[i+1];
+    }
+    
+    // find right edge
+    result.rightEdgeIndex = 127;
+    for(i=maxWhiteIndex;i<125;i++)
+    {
+        if(edgeGradients[i]>edgeThreshold)
+        {
+            result.rightEdgeIndex = i;
+            break;
+        }
+    }
+    
+    // find left edge
+    result.leftEdgeIndex = 0;
+    for(i=maxWhiteIndex;i>=2;i--)
+    {
+        if(edgeGradients[i] < -edgeThreshold)
+        {
+            result.leftEdgeIndex = i;
+            break;
+        }
+    }
+    
+    return result;
+}
+
+void Vision::lineScanLightAdjust()
+{
+    uint8_t i;
+
+    for(i=0;i<128;i++)
+    {
+        lineData[i] = (lineData[i]-150)*_vision_lightcomp[i];
+    }
+}
+
+void Vision::processTasks()
+{
+    if(newDataToProcess)
+    {
+        //TFC_BAT_LED3_ON;
+        if(lightCompensation)
+            lineScanLightAdjust();
+        currentRoadData = processLine();
+        newDataToProcess = false;
+        //TFC_BAT_LED3_OFF;
+    }
+}
+
+struct lineScanData Vision::getRoadData()
+{
+    return currentRoadData;
+}