【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;
   }

硬件连接

设置好串口,中断(具体详见代码)

完整代码

结果检测

传送门

  • 4
    点赞
  • 14
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 0
    评论
Pid小车直线行走代码主要使用了PID控制算法,通过测量车体的姿态误差,根据一定的控制算法计算出修正量,从而实现小车直线行走。下面是一个简单的示例代码: ```c #include <PID.h> // 定义PID参数 double Kp = 1.0; // 比例系数 double Ki = 0.5; // 积分系数 double Kd = 0.2; // 微分系数 // 定义目标位置和当前位置 double targetPos = 100.0; double currentPos = 0.0; // 定义P、I、D累计误差 double sumError = 0.0; double lastError = 0.0; // 定义限制输出范围 double maxOutput = 255.0; double minOutput = -255.0; // 创建PID控制对象 PID pid; void setup() { // 初始化PID控制对象 pid.init(Kp, Ki, Kd, &currentPos, &sumError, &lastError, maxOutput, minOutput); } void loop() { // 测量当前位置 currentPos = measureCurrentPosition(); // 调用PID控制算法计算修正量 double correction = pid.calculate(targetPos); // 输出修正量控制小车行走 drive(correction); } double measureCurrentPosition() { // 测量当前位置,例如读取编码器或者预测位置等 // 返回当前位置的测量值 double measuredPos = readEncoder(); return measuredPos; } void drive(double correction) { // 根据修正量控制小车行走,例如控制电机转速或舵机角度等 // 代码省略 } double readEncoder() { // 读取编码器的值或者通过其他方式获取当前位置的数值 // 代码省略 double encoderValue = 0.0; return encoderValue; } ``` 以上示例代码是一个简单的PID小车直线行走的实现,其中通过PID控制对象pid进行初始化,通过调用`pid.calculate(targetPos)`计算出修正量correction,然后根据修正量控制小车行走。其中,`measureCurrentPosition()`用于测量当前位置,`drive(correction)`用于控制小车行走。这样,通过不断调用`loop()`函数实现PID控制的小车直线行走。

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

go_bananas

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值