当前位置：物联沃-IOTWORD物联网 > 技术教程 > 2022年电赛C题：小车跟踪 – 方案1核心代码
代码收藏家代码收藏家 技术教程

2022年电赛C题：小车跟踪 – 方案1核心代码

目录

  • 前言
  • 一、题目
  • 二、方案1
  • 1、材料清单
  • 2、说明
  • 三、核心代码
  • 四、工程获取

    • 前言

          针对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("\r\n超声波检测距离：%d ",cm);
  printf("\r\n当前车速为：%d ",led0pwmval/9);	    //保持当提前的移动速度
	printf("\r\n当前Z坐标为：%f ",Tz_num);
	printf("\r\n当前X坐标为：%f ",Tx_num);
	printf("\r\n输出的PWM：%d ",led0pwmval);
	printf("\r\n输出的占空比：%d ",led0pwmval/9);
	if(qh_num<0) 
		printf("\r\nPID：-  %f.2 ",qh_num);
	if(qh_num>0)
		printf("\r\nPID：   %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("\r\n跟随ID1车牌id： %d ",id_flag);
					}	
						if(HC_cmd==13)
          {       
					id_flag=2;
					printf("\r\n跟随ID1车牌id： %d ",id_flag);
					}	
					if(HC_cmd==14)
					{       
					id_flag=3;
					printf("\r\n跟随ID1车牌id： %d ",id_flag);
					}	
					if(HC_cmd==15)
					{       
					id_flag=4;
					printf("\r\n跟随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("\r\n警告：检测物体贴近距离%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的分享

    物联沃分享整理
    物联沃-IOTWORD物联网 » 2022年电赛C题：小车跟踪 – 方案1核心代码

    代码收藏家 普通

    分享到：

    相关推荐

    发表回复