电磁寻迹智能车HAL库基于cubeMX—三轮（分段PID+归一化+差速+均值滤波+多路ADC+三叉+环岛+十字）

2023-04-16

amp left if

一、杂谈拖了好久才来更文章….是因为一直比较忙，哈哈。工程在文末今年呢，是第二次参加智能汽车校赛，本来也是参加了飞卡的，但是因为某些原因（包括个人的也有包括组队的一些其实现在看来也就那样的问题）我退出了，说有遗憾那必然是有的，因为毕竟哪个工科男生没有一个做车车的想法呢，但不后悔，因为有了更多时间去做

一、杂谈

拖了好久才来更文章…~~.是因为一直比较忙~~ ，哈哈。

工程在文末

今年呢，是第二次参加智能汽车校赛，本来也是参加了飞卡的，但是因为某些原因（包括个人的也有包括组队的一些其实现在看来也就那样的问题）我退出了，说有遗憾那必然是有的，因为毕竟哪个工科男生没有一个做车车的想法呢，但不后悔，因为有了更多时间去做其它也想做的事情。所以这个智能车校赛就当作过过车瘾了。

说一下大致的情况吧，我写程序调车，另一个同伴搭车做硬件，我们是高年级组了要求的是做三轮车，去年也参加了做的四轮车，去年调了一个月接近，也是我一个人调的程序，最后拿了三等奖。其实三轮车和四轮车区别不大，无非就改改代码控制而已。今年的三轮车组别，我调了一个通宵，最后拿了一等奖，因为平时实在太忙，没有什么时间来管这个，当然也是因为去年做过有点经验。

二、进入正题

其实，跑电磁组，完赛真的很简单，不管特殊元素，简单做一个PID的闭环就能完赛了。可以和飞卡的智能视觉做对比，完全不是一个量级的，飞卡智能视觉就算集三人之力要是实力和所花的时间不够，连完赛都做不到，大家可以去了解了解飞卡智能视觉，我看着那些大佬的视频，确实是佩服。RTT操作系统，动态路径规划，坐标点识别，包括一些蚁群算法等数据结构所用到的东西，目标检测模型训练很考验一个团队的综合实力。

好了，跑题了。

我们来看做这样一个电磁循迹三轮车所需要哪些硬件吧,以下是我选择的：
主控STM32F411CEU6单片机、多路电磁杆、三轮车模是向组织方购买的、电机驱动、OLED、干簧管，主要就是这些。

三、硬件、搭车

硬件其实挺简单的，对于这个比赛我对我们硬件员的要求搭车质量不高，只需要让他画个转接板，电磁杆直接用之前的（但其实这里比较草率了），到时直接把各个模块插到转接板上就行，硬件员还有点懵逼觉得自己的工作量是不是有点太少了，哈哈。

下面是原理图、PCB、实物图：

这些也不用多说，注意好预留的接口，没有用到的IO口引出来，干黄管的位置放在车底部为了触发灵敏，考虑好布局排布就欧克了。还有的话我这里是多加了陀螺仪MPU6050，是为了进环岛时的姿态判定，但其实后面时间不够来不及调了。

电磁杆：

画的六路，大致是个这样的摆放位置

其实吧，电磁杆是我们没有考虑好的，不应该这么随便，虽然这样的位置摆放的电磁杆也可以完赛，但很多特殊元素都很难判定，其实电磁最关键的就是利用电磁杆去感知赛道，你处在一个什么样的位置，是否有遇到特殊元素，比如环岛，三叉。所以电磁杆的制作就是最考验每个团队的观察分析能力了，其实我们第一次参加这个比赛的时候完全没有意识到电磁杆的重要性这么强，知道电磁杆很重要，但没有意识到它才是整个比赛的关键，有句话说得好，一个系统里面，硬件结构的搭建决定了整个系统的功能上限，因为软件调试永远离不开这些硬件，软件是驱动硬件的，硬件搞不好软件是没有办法的。

那么呢关于电磁杆我就点到为止，至于电感应该在电磁杆怎么摆放，什么方案最佳，就个人下去探索了，毕竟只有自己思考过的才是最适合自己的。

这里提供一下举个例子，比如我们之前是那种只有水平和竖直位置的摆放，当我们遇到了三叉应该怎么办呢。我们发现，如果中间再加一个水平电感，就会有只有在经过三叉时才有的效果了。因为这个水平电感在正常赛道时会一个切割沿线的电磁线，所以一直有一个电压产生，而当遇到三叉时，中间沿线的电磁线瞬间消失，而这就是三叉的特征，当具体还需要很多细节的处理，以保证判定准确。环岛也可以参照这种方法进行处理了。

可以看到我们上面的电磁杆也是临时改的，把其中一个水平电感放到了中间去。

最后的成品图：

四、软件部分

软件部分核心就是采集电感值，然后将电感值稍微进行一下滤波处理，其实硬件做的电磁感效果比较好的话也不需要怎么滤波。滤波后将实际值通过判断导入分段PID进行目标值的比较，我们将处理后的结果输出给电机执行，这就是整体的软件流程了，其实整体很简单。

另外的话在执行中再加上三叉，环岛等特殊元素的判定就欧克了。

cubeMX部分：

基础的配置，时钟等等就不再多说了，这里说一下 ADC多路配置吧
如图:

使能DMA模式

（1）Data Alignment—>Right alignment 此项选择右对齐，保持不变。

（2）Scan Conversion Mode—>Enable 此项选择扫描模式使能，代表对6路ADC输入分别扫描，如果不使能，其将会只读取一个输入的值。

（3）Continuous Conversion Mode —>Enable 此项选择连续扫描模式，表示将连续不断的对ADC的值进行转换。如果此项不使能，将会只采集一次就会停止，直到下一次使能才继续进行一次ADC转换。

（4）Discontinuous Conversion Mode—>Disable 此项和第三项是重复的。

（5）Number of Conversion---->6 此处有多少路输入就选择多少，而且只有在此处选择数字之后下面才会出来6个不同的通道。而且此处应该是在进入ADC1中第一个需要操作的步骤，否则（2）（3）是灰色的，无法选择使能。

使能一个定时器中断，用于一些功能的周期控制和按键的定时扫描

cubemx其他配置

使能了一些如驱动蜂鸣器的普通GPIO口，用的软件IIC，使能了硬件IIC用于驱动陀螺仪

代码部分

把用户代码单独封装出来

代码的初始化，其实这里mpu6050没有用到，初始化与否无所谓

while(w_mpu_init()!=mpu_ok)        
{
printf("ERROR\r\n");
HAL_Delay(5);
}
dmp_init();    
OLED_Init();
OLED_Clear();
data();
ADC_DMA_Iint();
HAL_TIM_Base_Start_IT(&htim2);
MOTO_init();

//HAL_Delay(100000);
PID_Init();
get_into('R');
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

主循环里面的代码执行

 /* USER CODE BEGIN WHILE */
  while (1)
  {
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
//读取mpu姿态
Yaw_angle();
//采集AD值
Ad_Value();
Direction_track();
//屏幕数据
Real_time_data();
if(last==1)
{
get_into('L');
}
//按键扫描
scan_key();
//PID处理

  }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

使用干簧管判定停车位这里：用的是标志位，用外部中断进行触发

//判断停车
if(stop_sta==0 && stop_falg0==0)
{
HAL_GPIO_WritePin(Bee_GPIO_Port,Bee_Pin,GPIO_PIN_SET);
stop_falg0=1;
stop_falg1=1;
}
if(stop_sta==1 && stop_falg1==1)
{
stop_falg2=1;
HAL_GPIO_WritePin(Bee_GPIO_Port,Bee_Pin,GPIO_PIN_RESET);
}
if(stop_falg2==1 && stop_sta==0)
{
last=1;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

判断环岛，这里是用到标志位加定时器中断计数，但由于整车只调了一个通宵，这里最后是没有调出来的，其实主要也和电磁杆的位置摆放和特殊点的分析不足有关。

//判断环岛
if(PID_direction.time==1)
{
PID_direction.count++;
if(PID_direction.count==800)
{
HAL_GPIO_WritePin(Bee_GPIO_Port,Bee_Pin,GPIO_PIN_RESET);
PID_direction.time=0;
PID_direction.count=0;
PID_direction.target_speed=70;
}
}
if(Left.firet>=800 && Left.second>=800 && Left.third>=800 && Right.firet <=100)     
{
PID_direction.time=1;
PID_direction.target_speed=30;
    }
   if(PID_direction.time==1)
   {
//HAL_GPIO_WritePin(Bee_GPIO_Port,Bee_Pin,GPIO_PIN_SET);
PID_direction.flag=2;
   }
   if(PID_direction.three_forks==1)
   {
if(PID_direction.into_three==1)//已经进入过一次了
{
//判定是环岛
PID_direction.cir=1;

TIM1->CCR1=30;  
TIM1->CCR2=60;                                                     
//TIM1->CCR1=20;                                                     //调试
//TIM1->CCR2=35;
}
else
{
TIM1->CCR1=45;                                                     //调试
TIM1->CCR2=10;
}
PID_direction.flag=3;
HAL_GPIO_WritePin(Bee_GPIO_Port,Bee_Pin,GPIO_PIN_SET);
}
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

判断三叉路，也是通过标志位和定时器的计数实现的，具体的思路前面也已经说过了，这里通过定时器计数判断主要是为了稳定过三叉，并且不误触发。

//判断三岔
if(PID_direction.three_forks==1)
{
PID_direction.count++;
if(PID_direction.count==20)          //调试
{
PID_direction.Kp = 0.022;//0.015
PID_direction.count=0;
PID_direction.three_forks=0;
PID_direction.target_speed=40;
PID_direction.run_time_three=1;
HAL_GPIO_WritePin(Bee_GPIO_Port,Bee_Pin,GPIO_PIN_RESET);
}
if(PID_direction.cir==1)
{
PID_direction.Kp = 0.023;
PID_direction.run_time_three=0;
PID_direction.target_speed=40;
PID_direction.timer++;
if(PID_direction.timer==250)
{
PID_direction.Kp = 0.015;
PID_direction.target_speed=70;
PID_direction.cir=0;
PID_direction.timer=0;
}
}
}
if(PID_direction.run_time_three==1)
{
//低速出三岔
PID_direction.timer++;
if(PID_direction.timer==580)
{
PID_direction.Kp = 0.015;
PID_direction.into_three++;
PID_direction.target_speed=70;
PID_direction.timer=0;
PID_direction.run_time_three=0;
PID_direction.three_on=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

以上代码的执行均放在了10ms一次的定时器中断回调函数内部

#include "ISR_CallBack.h"

KEY key[4]={0,0,0};
uchar stop_sta,stop_falg0=0,stop_falg1,stop_falg2;
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
if(htim->Instance == TIM2)
{
     //用户代码
     /*
     ......
     ......
     ......
     */
}
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

PID控制：

这里需要提到的是，某组织方提高的三轮车车模质量不敢恭维，没有编码器，要加编码器也要大改一下车身很麻烦，况且作为一个校赛，自己并没有花太多精力，于是直接开环跑了，校赛足矣。

电机的初始化

void MOTO_init(void)
{
HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_1);
HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_2);
HAL_GPIO_WritePin(STBY_GPIO_Port,STBY_Pin,GPIO_PIN_SET);

HAL_GPIO_WritePin(Moto1_GPIO_Port,Moto1_Pin,GPIO_PIN_SET);
HAL_GPIO_WritePin(Moto2_GPIO_Port,Moto2_Pin,GPIO_PIN_RESET);
HAL_GPIO_WritePin(Moto3_GPIO_Port,Moto3_Pin,GPIO_PIN_SET);
HAL_GPIO_WritePin(Moto4_GPIO_Port,Moto4_Pin,GPIO_PIN_RESET);
TIM1->CCR1=0;
TIM1->CCR2=0;
HAL_GPIO_WritePin(Bee_GPIO_Port,Bee_Pin,GPIO_PIN_RESET);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14

用的位置式PID足够了，进行了一个简单的判断分段

//水平电感
if(PID_direction.flag==1){
PID_direction.Pwm=PID_direction.Kp*PID_direction.err+PID_direction.Ki*PID_direction.integral+ \
PID_direction.Kd*(PID_direction.err-PID_direction.err_last);
}

//竖直电感
if(PID_direction.flag==2){
PID_direction.Pwm2=PID_direction.Kp2*PID_direction.err2+PID_direction.Ki2*PID_direction.integral2+ \
PID_direction.Kd2*(PID_direction.err2-PID_direction.err_last2);
}

PID_direction.err_last=PID_direction.err;
PID_direction.err_last2=PID_direction.err2;
1
2
3
4
5
6
7
8
9
10
11
12
13
14

PID的初始化

void PID_Init(void){

PID_direction.Kp = 0.015;//0.015
PID_direction.Ki = 0;
PID_direction.Kd = 0.075;//

PID_direction.integral=0;
PID_direction.time=0;
PID_direction.cir=0;
PID_direction.target_speed=70;
PID_direction.three_forks=0;
PID_direction.run_time_three=0;
PID_direction.three_on=0;
PID_direction.into_three=0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

限幅和取绝对值函数

uint16_t Limit(uint16_t pwm){
if (pwm <= 50){
return 50;
}
else if(pwm >= 950){
return 950;
}
else {
return pwm;
}
}

float Abs(float a){
if (a <= 0){
return -a;
}
else{
return a;
}
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

最后输入执行器，控制PWM

if(PID_direction.flag==1){
TIM1->CCR1=PID_direction.target_speed-PID_direction.Pwm;
TIM1->CCR2=PID_direction.target_speed+PID_direction.Pwm;
}
if(PID_direction.flag==2){
TIM1->CCR1=PID_direction.target_speed-PID_direction.Pwm2;
TIM1->CCR2=PID_direction.target_speed+PID_direction.Pwm2;
}
1
2
3
4
5
6
7
8

OLED实时显示个参数信息

void data(void)
{
OLED_ShowString(10,0,(uint8_t *)"Smart",16);
OLED_ShowString(55,0,(uint8_t *)"Car_MY",16);
OLED_ShowString(0,2,(uint8_t *)"V1:",12);
OLED_ShowString(0,3,(uint8_t *)"V2:",12);
OLED_ShowString(0,4,(uint8_t *)"V3:",12);
OLED_ShowString(0,5,(uint8_t *)"V4:",12);
OLED_ShowString(0,6,(uint8_t *)"V5:",12);
OLED_ShowString(0,7,(uint8_t *)"V6:",12);
}
void Real_time_data(void)
{
OLED_ShowNum(30,2,Left.firet,5,12); 

OLED_ShowNum(30,3,Left.second,5,12); 

OLED_ShowNum(30,4,Left.third,5,12); 

OLED_ShowNum(30,5,Right.third,5,12); 
//差与和的动态信息
OLED_ShowNum(30,6,Right.second,5,12);

OLED_ShowNum(30,7,Right.firet,5,12);

OLED_ShowFloat(67,6,mpu_pose_msg.yaw,3,2,12); 
}
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

对了，采集ADC的值进行了归一化如下：

#include "get_adc.h"

AD Left,Right;
uint32_t ADC_Value[6];
/* ===================================================================
备注：电感AD采集归一化计算
1、可以方便自己对数据的感知，在普通元素和特殊元素间；
2、在赛道更换后，测新的赛道的最大值，改变max的值即可，有较强的适应性；
3、方便数据处理。
* ===================================================================*/ 
*  //归一化到0-1000   
 Left.firet = 1000*(AD_average[5]-Min)/(Max-Min);               
 Left.second = 1000*(AD_average[4]-Min)/(Max-Min); 
 Left.third = 1000*(AD_average[3]-Min)/(Max-Min);  
 Right.third = 1000*(AD_average[2]-Min)/(Max-Min);  
 Right.second = 1000*(AD_average[1]-Min)/(Max-Min);  
 Right.firet = 1000*(AD_average[0]-Min)/(Max-Min);  
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17