Pascal Sandrez / Vision

Custom version for NXP cup car

Dependents:   NXPCUPcar

Vision.cpp@0:ccbc44580fab, 2016-03-25 (annotated)

Committer:
Clarkk
Date:
Fri Mar 25 13:05:09 2016 +0000
Revision:
0:ccbc44580fab
Child:
1:68bb92736e14
Initial version of vision library

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Clarkk 0:ccbc44580fab 1 #include "TFC.h"
Clarkk 0:ccbc44580fab 2 #include "Vision.h"
Clarkk 0:ccbc44580fab 3
Clarkk 0:ccbc44580fab 4 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};
Clarkk 0:ccbc44580fab 5
Clarkk 0:ccbc44580fab 6 Vision::Vision()
Clarkk 0:ccbc44580fab 7 {
Clarkk 0:ccbc44580fab 8 lightCompensation = false;
Clarkk 0:ccbc44580fab 9 newDataToProcess = false;
Clarkk 0:ccbc44580fab 10 }
Clarkk 0:ccbc44580fab 11
Clarkk 0:ccbc44580fab 12 void Vision::saveData(uint16_t* newData)
Clarkk 0:ccbc44580fab 13 {
Clarkk 0:ccbc44580fab 14 uint8_t i;
Clarkk 0:ccbc44580fab 15
Clarkk 0:ccbc44580fab 16 for(i=0;i<128;i++)
Clarkk 0:ccbc44580fab 17 {
Clarkk 0:ccbc44580fab 18 lineData[i] = newData[i];
Clarkk 0:ccbc44580fab 19 }
Clarkk 0:ccbc44580fab 20 newDataToProcess = true;
Clarkk 0:ccbc44580fab 21 }
Clarkk 0:ccbc44580fab 22
Clarkk 0:ccbc44580fab 23 struct lineScanData Vision::processLine()
Clarkk 0:ccbc44580fab 24 {
Clarkk 0:ccbc44580fab 25 uint32_t edgeProcessingBuffer[128];
Clarkk 0:ccbc44580fab 26 int16_t edgeGradients[128];
Clarkk 0:ccbc44580fab 27 uint8_t i;
Clarkk 0:ccbc44580fab 28 uint32_t maxWhite = 0;
Clarkk 0:ccbc44580fab 29 uint8_t maxWhiteIndex = 64;
Clarkk 0:ccbc44580fab 30 uint16_t edgeThreshold = 100;
Clarkk 0:ccbc44580fab 31 struct lineScanData result;
Clarkk 0:ccbc44580fab 32
Clarkk 0:ccbc44580fab 33 // uint32_t maxWhiteSum = 0;
Clarkk 0:ccbc44580fab 34 // uint8_t whiteAreaDelta = 5;
Clarkk 0:ccbc44580fab 35
Clarkk 0:ccbc44580fab 36 // remove noise from data
Clarkk 0:ccbc44580fab 37 for(i=2;i<126;i++)
Clarkk 0:ccbc44580fab 38 {
Clarkk 0:ccbc44580fab 39 // apply gaussian filter
Clarkk 0:ccbc44580fab 40 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
Clarkk 0:ccbc44580fab 41 if(edgeProcessingBuffer[i] > maxWhite)
Clarkk 0:ccbc44580fab 42 {
Clarkk 0:ccbc44580fab 43 maxWhite = edgeProcessingBuffer[i];
Clarkk 0:ccbc44580fab 44 maxWhiteIndex = i;
Clarkk 0:ccbc44580fab 45 }
Clarkk 0:ccbc44580fab 46 }
Clarkk 0:ccbc44580fab 47
Clarkk 0:ccbc44580fab 48 /* // Find the area of 2*whiteAreaDelta which is the brighter
Clarkk 0:ccbc44580fab 49 // init max white search
Clarkk 0:ccbc44580fab 50 maxWhiteSum = 0;
Clarkk 0:ccbc44580fab 51 for(i=2;i<(2+whiteAreaDelta*2);i++)
Clarkk 0:ccbc44580fab 52 {
Clarkk 0:ccbc44580fab 53 maxWhiteSum += edgeProcessingBuffer[i];
Clarkk 0:ccbc44580fab 54 }
Clarkk 0:ccbc44580fab 55 maxWhite = maxWhiteSum;
Clarkk 0:ccbc44580fab 56 maxWhiteIndex = 2+whiteAreaDelta;
Clarkk 0:ccbc44580fab 57 for(i=3+whiteAreaDelta;i<126-whiteAreaDelta;i++)
Clarkk 0:ccbc44580fab 58 {
Clarkk 0:ccbc44580fab 59 maxWhiteSum += edgeProcessingBuffer[i+whiteAreaDelta-1]-edgeProcessingBuffer[i-whiteAreaDelta-1];
Clarkk 0:ccbc44580fab 60 if(maxWhiteSum > maxWhite)
Clarkk 0:ccbc44580fab 61 {
Clarkk 0:ccbc44580fab 62 maxWhite = maxWhiteSum;
Clarkk 0:ccbc44580fab 63 maxWhiteIndex = i;
Clarkk 0:ccbc44580fab 64 }
Clarkk 0:ccbc44580fab 65 }*/
Clarkk 0:ccbc44580fab 66
Clarkk 0:ccbc44580fab 67 result.mostWhiteIndex = maxWhiteIndex;
Clarkk 0:ccbc44580fab 68 result.maxLightValue = (uint16_t)(maxWhite/49);
Clarkk 0:ccbc44580fab 69 edgeThreshold = maxWhite/20;
Clarkk 0:ccbc44580fab 70
Clarkk 0:ccbc44580fab 71 // find gradients
Clarkk 0:ccbc44580fab 72 for(i=2;i<125;i++)
Clarkk 0:ccbc44580fab 73 {
Clarkk 0:ccbc44580fab 74 edgeGradients[i] = edgeProcessingBuffer[i]-edgeProcessingBuffer[i+1];
Clarkk 0:ccbc44580fab 75 }
Clarkk 0:ccbc44580fab 76
Clarkk 0:ccbc44580fab 77 // find right edge
Clarkk 0:ccbc44580fab 78 result.rightEdgeIndex = 127;
Clarkk 0:ccbc44580fab 79 for(i=maxWhiteIndex;i<125;i++)
Clarkk 0:ccbc44580fab 80 {
Clarkk 0:ccbc44580fab 81 if(edgeGradients[i]>edgeThreshold)
Clarkk 0:ccbc44580fab 82 {
Clarkk 0:ccbc44580fab 83 result.rightEdgeIndex = i;
Clarkk 0:ccbc44580fab 84 break;
Clarkk 0:ccbc44580fab 85 }
Clarkk 0:ccbc44580fab 86 }
Clarkk 0:ccbc44580fab 87
Clarkk 0:ccbc44580fab 88 // find left edge
Clarkk 0:ccbc44580fab 89 result.leftEdgeIndex = 0;
Clarkk 0:ccbc44580fab 90 for(i=maxWhiteIndex;i>=2;i--)
Clarkk 0:ccbc44580fab 91 {
Clarkk 0:ccbc44580fab 92 if(edgeGradients[i] < -edgeThreshold)
Clarkk 0:ccbc44580fab 93 {
Clarkk 0:ccbc44580fab 94 result.leftEdgeIndex = i;
Clarkk 0:ccbc44580fab 95 break;
Clarkk 0:ccbc44580fab 96 }
Clarkk 0:ccbc44580fab 97 }
Clarkk 0:ccbc44580fab 98
Clarkk 0:ccbc44580fab 99 return result;
Clarkk 0:ccbc44580fab 100 }
Clarkk 0:ccbc44580fab 101
Clarkk 0:ccbc44580fab 102 void Vision::lineScanLightAdjust()
Clarkk 0:ccbc44580fab 103 {
Clarkk 0:ccbc44580fab 104 uint8_t i;
Clarkk 0:ccbc44580fab 105
Clarkk 0:ccbc44580fab 106 for(i=0;i<128;i++)
Clarkk 0:ccbc44580fab 107 {
Clarkk 0:ccbc44580fab 108 lineData[i] = (lineData[i]-150)*_vision_lightcomp[i];
Clarkk 0:ccbc44580fab 109 }
Clarkk 0:ccbc44580fab 110 }
Clarkk 0:ccbc44580fab 111
Clarkk 0:ccbc44580fab 112 void Vision::processTasks()
Clarkk 0:ccbc44580fab 113 {
Clarkk 0:ccbc44580fab 114 if(newDataToProcess)
Clarkk 0:ccbc44580fab 115 {
Clarkk 0:ccbc44580fab 116 //TFC_BAT_LED3_ON;
Clarkk 0:ccbc44580fab 117 if(lightCompensation)
Clarkk 0:ccbc44580fab 118 lineScanLightAdjust();
Clarkk 0:ccbc44580fab 119 currentRoadData = processLine();
Clarkk 0:ccbc44580fab 120 newDataToProcess = false;
Clarkk 0:ccbc44580fab 121 //TFC_BAT_LED3_OFF;
Clarkk 0:ccbc44580fab 122 }
Clarkk 0:ccbc44580fab 123 }
Clarkk 0:ccbc44580fab 124
Clarkk 0:ccbc44580fab 125 struct lineScanData Vision::getRoadData()
Clarkk 0:ccbc44580fab 126 {
Clarkk 0:ccbc44580fab 127 return currentRoadData;
Clarkk 0:ccbc44580fab 128 }