Pascal Sandrez / Vision

Custom version for NXP cup car

Dependents:   NXPCUPcar

Vision.cpp@1:68bb92736e14, 2017-06-01 (annotated)

Committer:
Clarkk
Date:
Thu Jun 01 13:17:03 2017 +0000
Revision:
1:68bb92736e14
Parent:
0:ccbc44580fab 
Few changes

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 1:68bb92736e14 16 dataAge.reset();
Clarkk 1:68bb92736e14 17 dataAge.start();
Clarkk 1:68bb92736e14 18
Clarkk 0:ccbc44580fab 19 for(i=0;i<128;i++)
Clarkk 0:ccbc44580fab 20 {
Clarkk 0:ccbc44580fab 21 lineData[i] = newData[i];
Clarkk 0:ccbc44580fab 22 }
Clarkk 0:ccbc44580fab 23 newDataToProcess = true;
Clarkk 0:ccbc44580fab 24 }
Clarkk 0:ccbc44580fab 25
Clarkk 0:ccbc44580fab 26 struct lineScanData Vision::processLine()
Clarkk 0:ccbc44580fab 27 {
Clarkk 0:ccbc44580fab 28 uint32_t edgeProcessingBuffer[128];
Clarkk 0:ccbc44580fab 29 int16_t edgeGradients[128];
Clarkk 0:ccbc44580fab 30 uint8_t i;
Clarkk 0:ccbc44580fab 31 uint32_t maxWhite = 0;
Clarkk 0:ccbc44580fab 32 uint8_t maxWhiteIndex = 64;
Clarkk 0:ccbc44580fab 33 uint16_t edgeThreshold = 100;
Clarkk 0:ccbc44580fab 34 struct lineScanData result;
Clarkk 0:ccbc44580fab 35
Clarkk 0:ccbc44580fab 36 // uint32_t maxWhiteSum = 0;
Clarkk 0:ccbc44580fab 37 // uint8_t whiteAreaDelta = 5;
Clarkk 0:ccbc44580fab 38
Clarkk 0:ccbc44580fab 39 // remove noise from data
Clarkk 0:ccbc44580fab 40 for(i=2;i<126;i++)
Clarkk 0:ccbc44580fab 41 {
Clarkk 0:ccbc44580fab 42 // apply gaussian filter
Clarkk 0:ccbc44580fab 43 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 44 if(edgeProcessingBuffer[i] > maxWhite)
Clarkk 0:ccbc44580fab 45 {
Clarkk 0:ccbc44580fab 46 maxWhite = edgeProcessingBuffer[i];
Clarkk 0:ccbc44580fab 47 maxWhiteIndex = i;
Clarkk 0:ccbc44580fab 48 }
Clarkk 0:ccbc44580fab 49 }
Clarkk 0:ccbc44580fab 50
Clarkk 0:ccbc44580fab 51 /* // Find the area of 2*whiteAreaDelta which is the brighter
Clarkk 0:ccbc44580fab 52 // init max white search
Clarkk 0:ccbc44580fab 53 maxWhiteSum = 0;
Clarkk 0:ccbc44580fab 54 for(i=2;i<(2+whiteAreaDelta*2);i++)
Clarkk 0:ccbc44580fab 55 {
Clarkk 0:ccbc44580fab 56 maxWhiteSum += edgeProcessingBuffer[i];
Clarkk 0:ccbc44580fab 57 }
Clarkk 0:ccbc44580fab 58 maxWhite = maxWhiteSum;
Clarkk 0:ccbc44580fab 59 maxWhiteIndex = 2+whiteAreaDelta;
Clarkk 0:ccbc44580fab 60 for(i=3+whiteAreaDelta;i<126-whiteAreaDelta;i++)
Clarkk 0:ccbc44580fab 61 {
Clarkk 0:ccbc44580fab 62 maxWhiteSum += edgeProcessingBuffer[i+whiteAreaDelta-1]-edgeProcessingBuffer[i-whiteAreaDelta-1];
Clarkk 0:ccbc44580fab 63 if(maxWhiteSum > maxWhite)
Clarkk 0:ccbc44580fab 64 {
Clarkk 0:ccbc44580fab 65 maxWhite = maxWhiteSum;
Clarkk 0:ccbc44580fab 66 maxWhiteIndex = i;
Clarkk 0:ccbc44580fab 67 }
Clarkk 0:ccbc44580fab 68 }*/
Clarkk 0:ccbc44580fab 69
Clarkk 0:ccbc44580fab 70 result.mostWhiteIndex = maxWhiteIndex;
Clarkk 0:ccbc44580fab 71 result.maxLightValue = (uint16_t)(maxWhite/49);
Clarkk 0:ccbc44580fab 72 edgeThreshold = maxWhite/20;
Clarkk 0:ccbc44580fab 73
Clarkk 0:ccbc44580fab 74 // find gradients
Clarkk 0:ccbc44580fab 75 for(i=2;i<125;i++)
Clarkk 0:ccbc44580fab 76 {
Clarkk 0:ccbc44580fab 77 edgeGradients[i] = edgeProcessingBuffer[i]-edgeProcessingBuffer[i+1];
Clarkk 0:ccbc44580fab 78 }
Clarkk 0:ccbc44580fab 79
Clarkk 0:ccbc44580fab 80 // find right edge
Clarkk 0:ccbc44580fab 81 result.rightEdgeIndex = 127;
Clarkk 0:ccbc44580fab 82 for(i=maxWhiteIndex;i<125;i++)
Clarkk 0:ccbc44580fab 83 {
Clarkk 0:ccbc44580fab 84 if(edgeGradients[i]>edgeThreshold)
Clarkk 0:ccbc44580fab 85 {
Clarkk 0:ccbc44580fab 86 result.rightEdgeIndex = i;
Clarkk 0:ccbc44580fab 87 break;
Clarkk 0:ccbc44580fab 88 }
Clarkk 0:ccbc44580fab 89 }
Clarkk 0:ccbc44580fab 90
Clarkk 0:ccbc44580fab 91 // find left edge
Clarkk 0:ccbc44580fab 92 result.leftEdgeIndex = 0;
Clarkk 0:ccbc44580fab 93 for(i=maxWhiteIndex;i>=2;i--)
Clarkk 0:ccbc44580fab 94 {
Clarkk 0:ccbc44580fab 95 if(edgeGradients[i] < -edgeThreshold)
Clarkk 0:ccbc44580fab 96 {
Clarkk 0:ccbc44580fab 97 result.leftEdgeIndex = i;
Clarkk 0:ccbc44580fab 98 break;
Clarkk 0:ccbc44580fab 99 }
Clarkk 0:ccbc44580fab 100 }
Clarkk 0:ccbc44580fab 101
Clarkk 1:68bb92736e14 102 result.leftEdgePosition = getPixelPosition(result.leftEdgeIndex);
Clarkk 1:68bb92736e14 103 result.rightEdgePosition = getPixelPosition(result.rightEdgeIndex);
Clarkk 1:68bb92736e14 104
Clarkk 1:68bb92736e14 105 result.age = dataAge;
Clarkk 1:68bb92736e14 106
Clarkk 0:ccbc44580fab 107 return result;
Clarkk 0:ccbc44580fab 108 }
Clarkk 0:ccbc44580fab 109
Clarkk 1:68bb92736e14 110 struct Point Vision::getPixelPosition(uint8_t pixelIndex)
Clarkk 1:68bb92736e14 111 {
Clarkk 1:68bb92736e14 112 struct Point position;
Clarkk 1:68bb92736e14 113 // lensToImageDistance = lensHeightFromFloor/cos(lensAngle)
Clarkk 1:68bb92736e14 114 // x = lensToImageDistance*(pixelSpace+pixelWidth)/focalDistance*(pixelIndex-63.5);
Clarkk 1:68bb92736e14 115 // x = 8.6396*(pixelindex-63.5);
Clarkk 1:68bb92736e14 116 // y = tan(lensAngle)*lensHeightFromFloor+wheelLensHorizontalDistance
Clarkk 1:68bb92736e14 117
Clarkk 1:68bb92736e14 118 position.x = 8.6396*((float)pixelIndex-63.5);
Clarkk 1:68bb92736e14 119 position.y = 303.266;
Clarkk 1:68bb92736e14 120
Clarkk 1:68bb92736e14 121 return position;
Clarkk 1:68bb92736e14 122 }
Clarkk 1:68bb92736e14 123
Clarkk 0:ccbc44580fab 124 void Vision::lineScanLightAdjust()
Clarkk 0:ccbc44580fab 125 {
Clarkk 0:ccbc44580fab 126 uint8_t i;
Clarkk 0:ccbc44580fab 127
Clarkk 0:ccbc44580fab 128 for(i=0;i<128;i++)
Clarkk 0:ccbc44580fab 129 {
Clarkk 0:ccbc44580fab 130 lineData[i] = (lineData[i]-150)*_vision_lightcomp[i];
Clarkk 0:ccbc44580fab 131 }
Clarkk 0:ccbc44580fab 132 }
Clarkk 0:ccbc44580fab 133
Clarkk 1:68bb92736e14 134 bool Vision::processTasks()
Clarkk 0:ccbc44580fab 135 {
Clarkk 0:ccbc44580fab 136 if(newDataToProcess)
Clarkk 0:ccbc44580fab 137 {
Clarkk 0:ccbc44580fab 138 //TFC_BAT_LED3_ON;
Clarkk 0:ccbc44580fab 139 if(lightCompensation)
Clarkk 0:ccbc44580fab 140 lineScanLightAdjust();
Clarkk 0:ccbc44580fab 141 currentRoadData = processLine();
Clarkk 0:ccbc44580fab 142 newDataToProcess = false;
Clarkk 0:ccbc44580fab 143 //TFC_BAT_LED3_OFF;
Clarkk 1:68bb92736e14 144 return true;
Clarkk 0:ccbc44580fab 145 }
Clarkk 1:68bb92736e14 146 return false;
Clarkk 0:ccbc44580fab 147 }
Clarkk 0:ccbc44580fab 148
Clarkk 0:ccbc44580fab 149 struct lineScanData Vision::getRoadData()
Clarkk 0:ccbc44580fab 150 {
Clarkk 0:ccbc44580fab 151 return currentRoadData;
Clarkk 0:ccbc44580fab 152 }