应用场景：
带舵机控制的超声测距雷达。舵机不停旋转，获得各个位置点距离，代码里体现为数组dist[],值为探测的距离，下标为当前舵机位置。
该算法获得足够宽度足够深度的一个提供小车前进的方向.代码是PC机上的模拟。
#define total_loc 10
unsigned char dist2[total_loc]=
{
1,2,3,4,5,6,7,8,9,10
};
unsigned char dist5[total_loc]=
{
8,7,7,7,3,4,7,8,9,10
};
unsigned char dist1[total_loc]=
{
10,9,8,7,6,5,4,3,2,1
};
unsigned char dist9[total_loc]=
{
1,1,1,1,1,1,1,1,1,1
};
unsigned char dist[total_loc]=
{
1,6,2,6,1,7,6,1,1,9
};
unsigned char dist3[total_loc]=
{
10,10,19,20,100,100,120,13,15,18
};
#define low_bar 4
#define high_bar 5
#include <stdio.h>
#define MINIMAL_SIZE 1
int calc_dir()
{
int start_loc = -1;
int end_loc = -1;
int breadth = 0;
int dir=-1;
int i;
for(i=0;i<total_loc; i++){
if(start_loc == -1 ){
if(dist[i] > high_bar){
start_loc = i;
printf("above bar %d\n",dist[i]);
}
}else{
if(dist[i] < low_bar){
end_loc = i;
printf("start loc is %d\n",start_loc);
printf("end loc is %d\n", end_loc);
if(end_loc - start_loc > breadth){ dir = (end_loc+start_loc)/2; breadth = end_loc-start_loc;}
start_loc = -1;
}
if(i == total_loc-1){
end_loc = total_loc-1;
printf("start loc is %d\n",start_loc);
printf("end loc is %d\n", end_loc);
if(end_loc - start_loc > breadth) { dir = (end_loc+start_loc)/2; breadth = end_loc-start_loc;}
start_loc = -1;
}
}
}
if(breadth <= MINIMAL_SIZE) dir = -1;
printf("final dir is %d\n",dir);
printf("final breadth is %d\n",breadth);
return dir;
}
int main()
{
calc_dir();
return 0;
}
|
发表于 2013-1-30 15:16:57
楼主
