2022电赛C题:小车跟踪(方案1+核心代码)

前言

      针对2022年电赛C题小车跟踪,本团队一共是做了两种方案:
      第一种主要以摄像头(openmv)为主,后车通过识别前车车上的二维码进行跟踪。这种方案,性能更稳定,兼容性更好,可以实现1-4小问。

具体可以参考openmv官网说明:

重点:https://book.openmv.cc/image/apriltag.html
在这里插入图片描述

      第二种,属于团队内部方案,暂时不公开,这种实现起来更简单,材料价格便宜,不过缺点就是,扩展性差,预计只能做到第三小问。
     这里讲一下,第一种方案的做法,并且在文底附上整个工程,有需要的自行下载(是用STM32做的,赛题要求TI板子,有基础的下载,做移植,没基础的慎重!)

一、题目

在这里插入图片描述
在这里插入图片描述

二、方案1

1、材料清单

在这里插入图片描述

调试的时候,看PID参数用,方便调试,无实际功能,只是调试。
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述

在这里插入图片描述
在这里插入图片描述
在这里插入图片描述
前车可以用循迹模块进行循迹行走,也可以用openmv,用openmv会复杂点,但是精度较高,本团队采用openmv进行巡线的。
在这里插入图片描述
电机驱动,相比于L298N,TB6612可以调PID
在这里插入图片描述

2、说明

时间比较赶,这里简单说一下思路,前车通过巡线走,后车扫描前车车尾的二维码,openmv将图像数据反馈给单片机,进行跟踪。

视频讲解二维码使用:
https://singtown.com/learn/49590/
在这里插入图片描述
首先进行摄像头初始化初始配置,然后开始检测二维码,为保证对二维码的准确判断,在初始化摄像头时,应该加入防止镜头畸变的初始配置,从而使图像边缘能够平顺展现。然后判断二维码中内容是否为系统的参数设置,如果是,则执行其对应追踪功能,如果不是,则返回继续检测有效二维码。追踪小车可实现 3 个功能。

①指定路径追踪功能。根据实际路况,通过距离、转弯角度、运行速度等 3 个参数设置,即可以实现对小车的路径控制;
② 指引路径追踪功能。以二维码内容单独实现小车的左转、右转、掉头、前进、后退等功能。实际操作时,可以在路口、转弯处张贴对应的二维码,当小车运动到该区域时识别到该处所贴的二维码,然后根据二维码执行对应动作,从而实现二维码规划小车运动路径。
③实时追踪功能。当 OpenMV 识别到的二维码内容既不是指定路径内容、也不是规划路径功能所用到的二维码内容,则会执行实时追踪该二维码的功能。

检测到二维码后,如果二维码不在整个图像的中心,那么就需要进行中心误差的计算,舵机云台的转向和小车驱动动作都与中心误差计算息息相关。获取二维码目标后,通过和图像的中心点做差,可以计算得出需要移动的 X、Y差值。然后再分别对 X、Y 分别调用 PID 算法,得到的结果就是对应舵机需要的旋转角度和小车转向的角度。在本文中,PID 控制器采用的数字位置式 PID 控制器,使追踪更稳定,且能自主追随二维码。另外,小车在运行时,PID 算法也为小车调速提供了依据。当二维码在图像正前方区域内时,小车执行前进指令,如果二维码距离摄像头越近,二维码成像面积会越大,此时小车距离二维码过近,小车速度应适当降低。当小车距离二维码越远时,成像也将越小,则小车速度应适当增加。

三、原理图
在这里插入图片描述

三、核心代码

#include "led.h"
#include "delay.h"
#include "key.h"
#include "sys.h"
#include "usart.h"
#include "beep.h"
#include "timer.h"
#include "oled.h"
#include "MOTIR.h"
#include "apiltag.h"
#include "math.h"

u16 led0pwmval=0;
u16 led0pwmval_l=0;
u16 led0pwmval_r=0;
u8 car_z; 
u8 car_x;
u8 car_pwm;
u8 zd_flag;
u16  pwm_max;
u16  pwm_small;  

extern u8 time3;

extern u8  TIM8CH1_CAPTURE_STA;		//输入捕获状态		    				
extern u16	TIM8CH1_CAPTURE_VAL;	//输入捕获值	

extern float Tx_num;
extern float Ty_num;
extern float Tz_num;
extern u8 apiltag_id;

extern u8 Tx_fu;
extern u8 Ty_fu;
extern u8 Tz_fu;
extern u8 hccmd_flag;
extern u8 oled_flag;
extern u8 openmv_usart1_flag;             //
extern u8 openmv_flag;     //作为标志物判断openmv是否实时传输数据,防止小车跑飞

float KP = 50;
float KI = 0;
float KD = 0;
float distance = 0;

float camera_distance=0;
float error = 0;
float last_error = 0;
float Output = 0;

float x_KP = 20;
float x_KI = 0;
float x_KD = 0;
float x_distance = 0;

float x_camera_distance=0;
float x_error = 0;
float x_last_error = 0;
float x_Output = 0;

float PID(float KP,float KI,float KD,float distance,float camera_distance)
{

	error = camera_distance - distance;
	Output = KP * (error) + KI * (error + last_error) + KD * (error - last_error);
	last_error = error;
	return Output;
}

float x_PID(float x_KP,float x_KI,float x_KD,float x_distance,float x_camera_distance)
{

	x_error = x_camera_distance - x_distance;
	x_Output = x_KP * (x_error) + x_KI * (x_error + x_last_error) + x_KD * (x_error - x_last_error);
	x_last_error = x_error;
	return x_Output;
}

 int main(void)
 {		
float qh_num=0;
float X_num=0;	 
float mm;
u16 cm;
u32 temp=0;
u16 num_pwm;

u8 id=0;
u8 id_flag=0;
u8 SRF05_flag=0; 
	 
u8 oled_hccmd=0;
u8 	PWMzkb;        
vu16 HC_cmd;           //此变量前缀vu16是为了调用在库函数中变量的return数值
	MOTIR_Init();        //电机初始化
	delay_init();	    	 //延时函数初始化	  
	LED_Init();		  		//将HY_SRF_04的触发IO初始化代替原有的LED初始化
	BEEP_Init();         	//初始化蜂鸣器端口
	delay_init();	    	 //延时函数初始化	  

	 
	 
	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
	 //设置NVIC中断分组2:2位抢占优先级,2位响应优先级
	uart4_init(115200);   //串口4初始化波特率为115200
	uart3_init(115200);	 //串口3初始化波特率为115200
 	LED_Init();			     //LED端口初	始化     
	KEY_Init();          //初始化与按键连接的硬件接口   此处注释掉会导致小车不动
 	TIM1_PWM_Init(899,3);	//定时器1主频为36M,计算为36000000/((899+1)*(3+1))=10KHZ
  TIM3_Int_Init(49,7199);	 //10Khz的计数频率,计数到500为500ms     //做中断计数
  TIM2_Int_Init(99,7199);	 //10Khz的计数频率,计数到500为500ms    //openmv中断提醒
	TIM8_Cap_Init(0XFFFF,36-1);	//以1Mhz的频率计数   
	 
	 OLED初始化设置
	 	OLED_Clear();     //OLED清除
	 OLED_Init();            //OLED初始化
	 OLED_ColorTurn(0);//0正常显示,1 反色显示
  OLED_DisplayTurn(0);//0正常显示 1 屏幕翻转显示
	 //推荐最低占空比为30%    由于本程序的转向代码通过更改两边电机的占空比取值相差40%,最小20%才能使小车前进,而20%会使小车在转向时一遍轮子彻底不动,摩檫力巨大,影响小车运作,因此推荐占空比30%,计算270/(899+1)=30%
    
		
	pwm_max=720;     //用于限制占空比最大值
	pwm_small=270;   //用于限制占空比最小值
	led0pwmval=pwm_small;     //初始化速度为最小值
 	while(1)
	{
		id=apiltag_id;
		PWMzkb=led0pwmval/9;        //定义PWM占空比变量给OLED显示
		oled_exe();                //显示中文函数  
   OLED_ShowNum(32,0,Tx_num,2,16);
		OLED_ShowNum(80,0,PWMzkb,2,16);
		OLED_ShowString(100,0,"id",16);
		OLED_ShowNum(104,16,apiltag_id,1,16);
		OLED_ShowNum(32,16,Ty_num,2,16);
		OLED_ShowString(52,16,"Ry",16);
	//OLED_ShowNum(80,16,Ry_num,2,16);
		OLED_ShowNum(32,32,Tz_num,2,16);
		OLED_ShowString(52,32,"flag",16);
		OLED_ShowNum(88,32,SRF05_flag,1,16);
		OLED_ShowNum(32,48,openmv_usart1_flag,3,16);  
		OLED_ShowString(0,48,"fps",16);
		
		OLED_ShowNum(88,48,oled_hccmd,2,16);
		OLED_ShowString(60,48,"cmd",16);
		OLED_Refresh();    //OLED刷新显示

if(hccmd_flag==1)
{
		HC_cmd=hc_cmd();      //此处不太正常hc_cmd()一直返回数值,而不是返回一次,可变,但一直发送
oled_hccmd=HC_cmd;
}hccmd_flag=0;
		
		///按键输入    此处返回K、led0pwmval数值
 
  //KEY_EXE();  
		
/		

  UART4_EXE();      //蓝牙串口驱动函数

 USART3_EXE();    //openmv数据处理

	
		
			

/		


if(HC_cmd==1|HC_cmd==2)
{
	zd_flag=1;
}
else if (HC_cmd!=1&HC_cmd!=0&HC_cmd!=2)
zd_flag=0;

if(HC_cmd==2)
{

 printf("rn超声波检测距离:%d ",cm);
  printf("rn当前车速为:%d ",led0pwmval/9);	    //保持当提前的移动速度
	printf("rn当前Z坐标为:%f ",Tz_num);
	printf("rn当前X坐标为:%f ",Tx_num);
	printf("rn输出的PWM:%d ",led0pwmval);
	printf("rn输出的占空比:%d ",led0pwmval/9);
	if(qh_num<0) 
		printf("rnPID:-  %f.2 ",qh_num);
	if(qh_num>0)
		printf("rnPID:   %f.2 ",qh_num);
	
}
if(HC_cmd==3)    //前
{
car_z=1;
car_x=0;
}
	if(HC_cmd==4)    //退
	{
	car_z=2;
car_x=0;
	}
		if(HC_cmd==5)    //左		
		car_x=1;
		
			if(HC_cmd==6)   //右
				car_x=2;
				if(HC_cmd==7)    //刹车
				{	
				car_z=3;
					car_x=0;
				}
			  	if(HC_cmd==8)
				car_pwm=1;
					if(HC_cmd==9)
       car_pwm=2;
	   			if(HC_cmd==10)
       led0pwmval=pwm_small;
					if(HC_cmd==11)
        led0pwmval=pwm_max;
					if(HC_cmd==12)
					{       
					id_flag=1;
					printf("rn跟随ID1车牌id: %d ",id_flag);
					}	
						if(HC_cmd==13)
          {       
					id_flag=2;
					printf("rn跟随ID1车牌id: %d ",id_flag);
					}	
					if(HC_cmd==14)
					{       
					id_flag=3;
					printf("rn跟随ID1车牌id: %d ",id_flag);
					}	
					if(HC_cmd==15)
					{       
					id_flag=4;
					printf("rn跟随ID1车牌id: %d ",id_flag);
					}	
	
						HC_cmd=0;

			//	指令判断
					
	if(id==id_flag)
			{
					if(openmv_flag==1)
					{
	 if(zd_flag==1)
	  {
		
	qh_num=PID(KP,KI,KD,30.5,Tz_num);
		if(qh_num<-pwm_max)
			qh_num=-pwm_max;
		if(qh_num>pwm_max)
			qh_num=pwm_max;
		
	X_num=x_PID(x_KP,x_KI,x_KD,2,Tx_num);
		if(X_num<-pwm_small)
			X_num=-pwm_small;
		if(X_num>pwm_small)
			X_num=pwm_small;
			
		if(qh_num>0)
		{
			num_pwm= (int)qh_num;
			
			
			MotorA1=0;   // 左轮前进
			MotorA2=1;
			MotorC1=1;
			MotorC2=0;
     
			TIM_SetCompare1(TIM1,num_pwm+X_num);		        //占用比等于led0pwmval/arr+1    此处为定时器PWM输出通道1
			TIM_SetCompare3(TIM1,num_pwm+X_num);		        //占用比等于led0pwmval/arr+1    此处为定时器PWM输出通道3
		}
				 
		if(qh_num<0)
		{	
			num_pwm= (int)-qh_num;
			MotorA1=1;   // 左轮后退
			MotorA2=0;
			MotorC1=0;
			MotorC2=1;
			
			TIM_SetCompare1(TIM1,num_pwm-X_num);		        //占用比等于led0pwmval/arr+1    此处为定时器PWM输出通道1
			TIM_SetCompare3(TIM1,num_pwm-X_num);		        //占用比等于led0pwmval/arr+1    此处为定时器PWM输出通道3
		}
					
		if(qh_num>0)
		{
			num_pwm= (int)qh_num;
			MotorB1=0;    //右轮前进
			MotorB2=1;
			MotorD1=1;
			MotorD2=0;
      
			TIM_SetCompare2(TIM1,num_pwm-X_num);		        //占用比等于led0pwmval/arr+1    此处为定时器PWM输出通道1
			TIM_SetCompare4(TIM1,num_pwm-X_num);		        //占用比等于led0pwmval/arr+1    此处为定时器PWM输出通道3
		}

		if(qh_num<0)
		{
			num_pwm= (int)-qh_num;
			MotorB1=1;    //右轮后退
			MotorB2=0;
			MotorD1=0;
			MotorD2=1;

			TIM_SetCompare2(TIM1,num_pwm+X_num);		        //占用比等于led0pwmval/arr+1    此处为定时器PWM输出通道1
			TIM_SetCompare4(TIM1,num_pwm+X_num);		        //占用比等于led0pwmval/arr+1    此处为定时器PWM输出通道3
		}
	
       }
	   }
					
		 	if(openmv_flag==0)
			{
				MotorA1=0;   
			  MotorA2=0;
			  MotorC1=0;
			  MotorC2=0;
				MotorB1=0;   
			  MotorB2=0;
			  MotorD1=0;
			  MotorD2=0;
			}
		}
			else if(id!=id_flag)
			{
				MotorA1=0;   
			  MotorA2=0;
			  MotorC1=0;
			  MotorC2=0;
				MotorB1=0;   
			  MotorB2=0;
			  MotorD1=0;
			  MotorD2=0;
			}
	/*
电机驱动函数 
	car_z   控制前进后退
	car_x   控制左右转
	car_pwm   控制加减速
	*/
	/
if(zd_flag==0)
{
Motor_EXE(car_z,car_x,car_pwm,pwm_max,pwm_small);     
TIM_SetCompare1(TIM1,led0pwmval_l);		        //占用比等于led0pwmval/arr+1    此处为定时器PWM输出通道1
TIM_SetCompare3(TIM1,led0pwmval_l);		        //占用比等于led0pwmval/arr+1    此处为定时器PWM输出通道3
	
TIM_SetCompare2(TIM1,led0pwmval_r);		        //占用比等于led0pwmval/arr+1    此处为定时器PWM输出通道2
TIM_SetCompare4(TIM1,led0pwmval_r);		        //占用比等于led0pwmval/arr+1    此处为定时器PWM输出通道4
}
	
///				
	PCout(7)=1;
	delay_us(10);
	PCout(7)=0;


		if(TIM8CH1_CAPTURE_STA&0X80)//成功捕获到了一次上升沿
		{
			temp=TIM8CH1_CAPTURE_STA&0X3F;
			temp*=65536;//溢出时间总和
			temp+=TIM8CH1_CAPTURE_VAL;//得到总的高电平时间
			
			mm=temp*0.34/4;           //单位mm     超声波测距范围2cm~450cm,最高精度单位3mm
			cm=mm/10;

			if(mm>20&&cm<4500)
			{
		if(mm<150)
		{	SRF05_flag=1;
			if(time3==5)   //等待500ms,仍在就代表无误判
			{
    printf("rn警告:检测物体贴近距离%d cm",cm);
		time3=0;
			}
		}
		if(mm>150)
			SRF05_flag=0;
		}
			TIM8CH1_CAPTURE_STA=0;//开启下一次捕获
		}
   


	}
}


四、工程获取

20年广东电赛开放题:本团队做的小车跟踪,刚好吻合2022电赛题目,所有资料(完成工程+原理图等),都集中在这里了,时间赶,还没整理,介意的不要下载。
STM32F103+openmv4+码盘电机(有基础的可以移植到TI板子)

链接:https://pan.baidu.com/s/1Hof4heUnRhtKbP4Xq-0n_g?pwd=FBMZ
提取码:FBMZ
–来自百度网盘超级会员V5的分享
在这里插入图片描述

本图文内容来源于网友网络收集整理提供,作为学习参考使用,版权属于原作者。
THE END
分享
二维码

)">
< <上一篇
下一篇>>