#include "LindarPos.h"
|
#include "OrdLidar.h"
|
#include "KMachine.h"
|
|
int ProcessPos(int nLidarIdx, stLidarDot * pLindarDots, int nCount)
|
{
|
int nValidCount0 = nCount;
|
int minDisIndex = 0; int minDistance = 55555;
|
int minZ = 10000; int minZIndex= -1;
|
int planeCount =0; // ×îµ×²ãƽÃæµÄµãÊýÁ¿¡£
|
int firstmin = 1000,firstmax = -1000;
|
int secondmin = 1000, secondmax = -1000;
|
|
int firstCount=0; int secondCount =0; int midCount =0;
|
results[0]=0;
|
// Çó minZ ºÍ minDistance;
|
for (int j = 0;j < nValidCount0;j++){
|
if (pLindarDots[j].distance < 50) continue; // skip too small points;
|
if (pLindarDots[j].y < 40) continue; // skip too small points;
|
if (pLindarDots[j].x < -200 || pLindarDots[j].x > 500) continue;
|
if (pLindarDots[j].y < minZ) { minZIndex = j; minZ = pLindarDots[j].y; }
|
if (pLindarDots[j].distance < minDistance) {
|
minDisIndex = j; minDistance = pLindarDots[j].distance;
|
}
|
}
|
|
for (int j = nValidCount0 -1 ;j>=0 && minZ < 500;j--)
|
{
|
int x = pLindarDots[j].x;
|
int y = pLindarDots[j].y;
|
int d = pLindarDots[j].distance;
|
|
if (d < 50) continue; // skip too small points;
|
if (y < 40) continue; // skip too small points;
|
if (x < -200 || x > 500) continue;
|
// Ö»²éÕÒ µ×²¿ 50mm Êý¾Ý
|
|
//results[0]++;
|
if (y >= minZ + 50) {
|
if ((firstCount >0 && secondCount == 0)
|
|| (secondCount >0 && firstCount == 0)) {
|
midCount++;
|
}
|
continue;
|
}
|
|
if (planeCount < 40) {
|
KMem.WDT[40+2 * planeCount]=d;
|
KMem.WDT[40+2 * planeCount +1 ]=x;
|
}
|
|
planeCount++;
|
if (x > -200 && x < 500)
|
{
|
if (x < firstmin) firstmin = x;
|
if (x > secondmax) secondmax = x;
|
}
|
// ºóÑØ
|
if (x > -200 && x < 100 && y < minZ + 40 )
|
{
|
if (x > firstmax) firstmax = x;
|
firstCount++;
|
}
|
// Ç°ÑØ
|
if (x > 200 && x < 500 && y < minZ + 40 )
|
{
|
if (x < secondmin) secondmin = x;
|
secondCount ++;
|
}
|
|
}
|
results[0]=planeCount;
|
results[1]=firstCount;
|
results[2]=midCount;
|
results[3]=secondCount;
|
|
results[4]=firstmin;
|
results[5]=firstmax;
|
results[6]=secondmin;
|
results[7]=secondmax;
|
int avg=0;
|
if (firstCount > 0 && secondCount > 0 && midCount > 0 ){
|
avg = (secondmin + firstmax) / 2;
|
}else if (planeCount>10) {
|
avg = (firstmin + secondmax) /2;
|
}
|
|
nPosX = (nPosX *3 + avg) /4;
|
if (minZ < 9999) nPosZ1 = (minZ + nPosZ1 * 3)/4;
|
return 0;
|
|
|
|
}
|
