2022电赛C题：小车跟踪（方案1+核心代码）
Posted 千歌叹尽执夏
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了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;//开启下一次捕获
四、工程获取
20年广东电赛开放题：本团队做的小车跟踪，刚好吻合2022电赛题目，所有资料（完成工程+原理图等），都集中在这里了，时间赶，还没整理，介意的不要下载。
STM32F103+openmv4+码盘电机（有基础的可以移植到TI板子）
链接：https://pan.baidu.com/s/1Hof4heUnRhtKbP4Xq-0n_g?pwd=FBMZ
提取码：FBMZ
–来自百度网盘超级会员V5的分享
以上是关于2022电赛C题：小车跟踪（方案1+核心代码）的主要内容，如果未能解决你的问题，请参考以下文章
2021电赛F题送药小车视觉部分的一种思路（双OpenMV法）