【stm32】利用单片机实现小车直线和圆弧的识别

特别感谢皎皎的大力帮助

项目要求

项目分析

要解决这个问题我们可以把这个项目划分为以下几个板块

解决步骤

数据接收：

我们在这次的项目中用的雷达是RPLIDAR A1的雷达

他的接收数据的格式为：

所以我们把线接好：如图：

然后我们就可以接收到数据了：
这是原始数据：

可以发现这是非常杂乱无章的，我们把这个数据处理和解算一下：
我们用以下代码来处理数据：

 while(1) 			
	{	
		if(mark)break;
		if(jyj){//接收到数据：
		if(flag==1)
		{
			RX_buffer[count]=m;
			count++;
	 		if(count==4){
				if(!(RX_buffer[0]&0x01)){
					flag=0;count=0;
				}
				angle2=(RX_buffer[1]<<1);
				distance=(RX_buffer[3]<<6|RX_buffer[2]>>2);
				
				if(distance!=0&&count!=0){//记录数据
					angle=(RX_buffer[1]<<1);
					dis=(RX_buffer[3]<<6|RX_buffer[2]>>2);
					tot=tot+1;
					JD[angle]=dis;
					if(tot>650)mark=1;//扫描的点数
				}
				flag=0;
				count=0;
			}
		}
			if(flag==0)//点的检测标志
			{				
				if(m==0x3E)//检测到点
				{
					flag=1;
				}
			}
			jyj=0;
		}
	}

然后我们得到处理好的数据：

算法设计

有了数据我们就需要设计算法来判断直线和曲线了。
我设计的算法的大致的流程如下：

• 得到数据点集：存储一个数组，下标为角度，数值为方向
• 每三度计算一次平均值，去除噪点
• 在去除噪点的数据上，每三个数据计算一次斜率，若差值小则判断为直线，否则为曲线
• 对于每个判断的直线或曲线，在相应的方向加减权值
• 小车每次往权值最大的方向运动
• 反复执行上述代码

具体代码：

for(j=1;j<120;j++){//判断直线和弧线
   	if(res[j-1]==0||res[j]==0||res[j+1]==0)continue;
   	k1=(sin((j-1)*3*pi)*res[j-1]-sin(j*3*pi)*res[j])/(cos((j-1)*3*pi)*res[j-1]-cos(j*3*pi)*res[j]);
   	k2=(sin(j*3*pi)*res[j]-sin((j+1)*3*pi)*res[j+1])/(cos(j*3*pi)*res[j]-cos((j+1)*3*pi)*res[j+1]);
   	if(k1>=k2)cha=k1-k2;
   	else cha=k2-k1;
   	if(cha>0.38)continue;
   	if( cha<0.12 ){
   		theta=j*3;
   		d=res[j];
   		Add(j,0);
   	}
   	else{
   		theta=j*3;
   		d=res[j];
   		Add(j,1);
   	}
   }
   //判断哪个方向
   forward[3]=-1000;
   for(j=1;j<=4;j++){
   	if(forward[j]>Max)Max=forward[j];
   }
   //小车运行：
   for(j=1;j<=4;j++){
   		if(forward[j]==Max){
   			if(j==1)USART_SendData(USART1,0x01);
   			if(j==2)USART_SendData(USART1,0x02);
   			if(j==3)USART_SendData(USART1,0x03);
   			if(j==4)USART_SendData(USART1,0x04);
   		}
   		forward[j]=0;
   }

硬件连接

设置好串口，中断（具体详见代码）

完整代码

结果检测

传送门