• 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("\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;//开启下一次捕获
		}
   


	}
}



    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
    • 45
    • 46
    • 47
    • 48
    • 49
    • 50
    • 51
    • 52
    • 53
    • 54
    • 55
    • 56
    • 57
    • 58
    • 59
    • 60
    • 61
    • 62
    • 63
    • 64
    • 65
    • 66
    • 67
    • 68
    • 69
    • 70
    • 71
    • 72
    • 73
    • 74
    • 75
    • 76
    • 77
    • 78
    • 79
    • 80
    • 81
    • 82
    • 83
    • 84
    • 85
    • 86
    • 87
    • 88
    • 89
    • 90
    • 91
    • 92
    • 93
    • 94
    • 95
    • 96
    • 97
    • 98
    • 99
    • 100
    • 101
    • 102
    • 103
    • 104
    • 105
    • 106
    • 107
    • 108
    • 109
    • 110
    • 111
    • 112
    • 113
    • 114
    • 115
    • 116
    • 117
    • 118
    • 119
    • 120
    • 121
    • 122
    • 123
    • 124
    • 125
    • 126
    • 127
    • 128
    • 129
    • 130
    • 131
    • 132
    • 133
    • 134
    • 135
    • 136
    • 137
    • 138
    • 139
    • 140
    • 141
    • 142
    • 143
    • 144
    • 145
    • 146
    • 147
    • 148
    • 149
    • 150
    • 151
    • 152
    • 153
    • 154
    • 155
    • 156
    • 157
    • 158
    • 159
    • 160
    • 161
    • 162
    • 163
    • 164
    • 165
    • 166
    • 167
    • 168
    • 169
    • 170
    • 171
    • 172
    • 173
    • 174
    • 175
    • 176
    • 177
    • 178
    • 179
    • 180
    • 181
    • 182
    • 183
    • 184
    • 185
    • 186
    • 187
    • 188
    • 189
    • 190
    • 191
    • 192
    • 193
    • 194
    • 195
    • 196
    • 197
    • 198
    • 199
    • 200
    • 201
    • 202
    • 203
    • 204
    • 205
    • 206
    • 207
    • 208
    • 209
    • 210
    • 211
    • 212
    • 213
    • 214
    • 215
    • 216
    • 217
    • 218
    • 219
    • 220
    • 221
    • 222
    • 223
    • 224
    • 225
    • 226
    • 227
    • 228
    • 229
    • 230
    • 231
    • 232
    • 233
    • 234
    • 235
    • 236
    • 237
    • 238
    • 239
    • 240
    • 241
    • 242
    • 243
    • 244
    • 245
    • 246
    • 247
    • 248
    • 249
    • 250
    • 251
    • 252
    • 253
    • 254
    • 255
    • 256
    • 257
    • 258
    • 259
    • 260
    • 261
    • 262
    • 263
    • 264
    • 265
    • 266
    • 267
    • 268
    • 269
    • 270
    • 271
    • 272
    • 273
    • 274
    • 275
    • 276
    • 277
    • 278
    • 279
    • 280
    • 281
    • 282
    • 283
    • 284
    • 285
    • 286
    • 287
    • 288
    • 289
    • 290
    • 291
    • 292
    • 293
    • 294
    • 295
    • 296
    • 297
    • 298
    • 299
    • 300
    • 301
    • 302
    • 303
    • 304
    • 305
    • 306
    • 307
    • 308
    • 309
    • 310
    • 311
    • 312
    • 313
    • 314
    • 315
    • 316
    • 317
    • 318
    • 319
    • 320
    • 321
    • 322
    • 323
    • 324
    • 325
    • 326
    • 327
    • 328
    • 329
    • 330
    • 331
    • 332
    • 333
    • 334
    • 335
    • 336
    • 337
    • 338
    • 339
    • 340
    • 341
    • 342
    • 343
    • 344
    • 345
    • 346
    • 347
    • 348
    • 349
    • 350
    • 351
    • 352
    • 353
    • 354
    • 355
    • 356
    • 357
    • 358
    • 359
    • 360
    • 361
    • 362
    • 363
    • 364
    • 365
    • 366
    • 367
    • 368
    • 369
    • 370
    • 371
    • 372
    • 373
    • 374
    • 375
    • 376
    • 377
    • 378
    • 379
    • 380
    • 381
    • 382
    • 383
    • 384
    • 385
    • 386
    • 387
    • 388
    • 389
    • 390
    • 391
    • 392
    • 393

    四、工程获取

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

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

  • 相关阅读:
    JVM阶段（4）-回收策略
    进程的虚拟地址空间
    CVPR2022-不对称分辨率图像的立体匹配
    R2R 的一些小tip
    JVM完整图文学习笔记 (含拓展知识广度学习) 第三章: 类加载与字节码技术
    MyBatis工作原理
    [C++](20)红黑树，调整规则图解，插入功能代码实现
    算法leetcode｜剑指 Offer 27. 二叉树的镜像｜226. 翻转二叉树（rust很强）
    Home Assistant使用ios主题更换背景
    清理Maven仓库中下载失败的文件
  • 原文地址：https://blog.csdn.net/weixin_46423500/article/details/126028226