2024电赛H题_自动行驶小车

本文档讲到的有些基础概念大家如果不懂的，需要自己去查资料。有些基础操作和模块使用方法，例如利用TB6612电机驱动去驱动小车动起来，网上有非常多的教程，本文就不详细阐述这些东西了，但是有讲到的一些东西的也会放一些指向链接去供大家去参考学习。另外，本文只是说明了当时设计小车时使用的方法与思路，不代表最佳方案，本文的方案也还有很大的改进空间。本文列举的所有代码和示例是本人比赛时编写的，仅供参考，不一定能直接套用。

一、器件准备

小车

两轮驱动的轮式小车，长宽要符合题目要求，一般是两个轮子，带一个万向轮的三轮小车，本次小车我没用到编码器，故有无编码器无所谓。（可以用编码器来计算距离提高容错，但是我没用）

电源

1. 12V电池：因为电机转动需要的电流比较大，最好是输出电流达2A以上的电池最好。

2. DC-DC开关降压模块：将12V电压降到5V，给5V的模块供电。

3. LDO线性稳压模块：将5V降压至3.3V，一般单片机开发板上有自带，也就是板子上的5V引脚就是自带LDO稳压的输入脚。

关于DC-DC和LDO有关的概念可参考：线性稳压模块自制或自行去搜索相关资料了解，在这里不做过多阐述，其实就是要搞降压模块就行了，DC-DC开关电源在电压差较大或大电流的时候没这么会发热，所以先用DC-DC稳压降到5V，然后再用LDO线性稳压模块降到3.3V。

其他器件

1. 德州仪器TI MSPM0G3507 相关：

   题目限定MSPM0系列，最高频率达80MHz，用来当小车的主控单片机，输入电压范围1.62V-3.6V，再高就容易烧。

2. 电机驱动：TB6612 相关:

   小体积电机驱动模块，最高支持100Khz频率的PWM波调速，一个可以驱动两路电机，最大支持3A电流，就用这个。别用L298N，那个容易出问题，而且PWM频率高起来就不行了。

   使用TB6612电机驱动可以用单片机来控制小车轮子的正转反转，也可以用PWM波的占空比来分别对小车的两个轮子进行转速调节，从而控制小车的速度。通过设置左右轮不同的转速，可以进行差速转弯。

3. 陀螺仪：JY61P 相关:

   维特智能的六轴陀螺仪传感器JY61P，可以用串口直接接收当前的角速度和角度，里面自带了滤波算法和电路，不需要跟MPU6050一样自己写算法去滤波等等，比较方便，并且MPU6050特别不好用，误差也非常大，不建议使用。本次只需要用到Z轴航向角即可，其他的量不需要用到，有钱可以去买维特的HWT101测航向角专用的,这个精度更高零漂更小，但是JY61P实测也是能用的，有点误差可以用小算法纠正。

   使用陀螺仪模块来获取当前小车的角度信息，把陀螺仪固定在小车上，即可通过陀螺仪来获取小车当前的角度信息。

4. 灰度传感器

   我用的是7路的灰度传感器，灰度传感器一般5路及以上就行了，最好用多路一点的，因为更宽，在没有线的路段转入有线的路段的时候更容易碰到黑线。灰度很简单，就是探灯的底下黑线就输出低电平，白线就输出高电平，在此不过多阐述。

   利用灰度传感器来进行黑线的识别和循迹，以及小车当前底下黑白的判断。

5. 2个按键、LED灯与蜂鸣器

   我只用了两个按键，一个按键用来切换题目，另一个按键用来启动，也可以准备多个按键，看自己。可以自己准备洞洞板焊接按键和排针，或者用现成的按键模块，当然会打板的直接打板焊就更好了。LED灯和蜂鸣器也可以直接接在车上，使用单片机的IO口高低电平进行控制。

   使用按键来作为小车的启动触发按钮与不同题目之间的切换按钮，LED与蜂鸣器用来实现声光提示功能。

6. 激光笔

   激光笔是用来帮助我们放车的时候对齐的，要绑在小车正前方固定死，并且角度要刚刚好对着正前方，如果肉眼能够对齐，不需要激光笔也可以。

如图所示，地图就是两个圆弧组成的简单的图，两个圆弧的边缘四个点分别为ABCD，我们要控制小车按题目的要求在地图上跑。

二、硬件连接

在硬件连接上，部分灰度传感器输出的高电平信号为5V，如果直接将其连接到MSPM0G3507的IO口容易烧芯片，因为MSPM0G3507芯片除了那两个PA0、PA1开漏引脚，其他引脚都没有5V耐压，这一点要注意。

三、题目解读与思路确定

第1题：将小车放在位置 A 点，小车能自动行驶到 B 点停车，停车时有声光提示。（15s）

题目一思路

首先小车从A点到B点走直线这一段：一开始我们将小车放置在A点，正前方需要正对着B点（要对的准可以借助激光来对）。

简单来说就是，我们将小车放在A点并对准B点后，将当前陀螺仪得到的角度作为我们需要一直保持的角度先记录下来，然后两个轮子设定一个正常速度开始走（两个轮子PWM波可以一样），然后一直去获取新的角度与刚开始记录的角度进行对比，如果角度与刚开始的角度有偏差了，则马上控制电机差速去调整回原来的角度，如此往复。

例子：我们将小车放置在A点后并对准B点后，陀螺仪获取的Z轴角度为31°，则此时将31°记录到变量中，而后设置两个电机PWM波占空比均为50%，此时小车会向前走，然后开始持续记录陀螺仪的实时Z轴角度。当陀螺仪Z轴角度变化到32°时，说明小车向左偏移了1°，此时应该将左轮PWM增加，右轮PWM减少，让小车往右转一点点，调回去。当陀螺仪Z轴角度变化到30°时，说明小车向右偏移了1°，此时应该将右轮PWM增加，左轮PWM减少，让小车往左转一点点，如此往复，让小车的角度一直保持在刚开始记录的31°左右，即可实现小车直线走。

当有偏差的时候PWM差值应该给多少？

需要根据偏差角度来调，偏差越大，则两轮之间的差值给越大，例如向左偏了x°，则两个轮子PWM之间的差值可以给一个10x或者20x，也就是可以把角度偏差值乘以一个比例系数P来确定两轮之间的PWM差速。（只有一个比例系数的反馈调节，相当于PID调节里面只有P参数的调节，具体的比例系数需要根据真实情况去自己调，可以初步让小车走直线）

如何判断小车已经从直线走到了B点？

在小车直走的过程中不断地去读取灰度传感器的数值，当任意一路读取到底下有黑线时，即可判断当前已经到达B点，此时直接停止电机转动与直线走的状态，并同时将LED灯与蜂鸣器启用0.5S后关闭，即可停下。（为了防止小车在走直线中由于地图上的黑点或者灰度传感器的误差产生的黑点从而导致误检测，可设置一段时间内连续检测到好几次黑点才判断到B点停车）

第一题总结

1. 小车初始放置在A点，正角度对着B点，可借助激光来对正。

2. 小车通过陀螺仪的角度反馈去控制电机始终保持一个角度直走。（陀螺仪角度反馈的直线PID调节）

3. 小车通过灰度传感器去检查是否到了B点（黑线），检测到了直接停车结束之前的状态并产生声光提示即可。

整个过程只涉及到读取陀螺仪Z轴的角度值与根据角度值偏差来控制电机转向让小车走直线，有兴趣的可以去细学PID算法，自己去再增加D值去抑制P比例系数的过响应，让小车走直线走的更稳定，本过程算是在小车行走过程中的PID转向环的调节。

PID小Tips：一般PID算法的代码都要在定时器中断函数里面去运行，如读取传感器的值并进行PID运算后得出控制量再控制电机这一个逻辑过程需要在定时器中断里面写，并且每计算一次的时间间隔要相同，比如定时器设定10ms触发一次中断，那PID就是每10ms进行一次数据读取和运行。建议不要在while里面写，因为while每次执行的时间间隔不固定，会被受执行的代码所影响。定时器中断内一定不要运行耗时的代码，例如延时函数，打印输出信息，大量循环递归等，否则易导致中断重入不触发或者同优先级冲突。

第2题：将小车放在位置 A 点，小车能自动行驶到 B 点后，沿半弧线行驶到 C点，再由 C 点自动行驶到 D 点，最后沿半弧线行驶到 A 点停车，每经过一个点，声光提示一次。（30S）

题目二思路

首先从A点到B点这段，我们按照题目一的方法来正常走完就可以了，接下来到达B点后检测到黑线存在，随即产生声光提示，接着开始就不需要角度反馈了，直接进入循迹模式，电机也设定一定的初始速度，通过读取灰度传感器的值去进行小车巡线。首先我们要掌握小车是如何用灰度传感器循迹的，不懂的请转入多路灰度传感器PID循迹学习，先把正常的循迹调好，能够沿着圆弧循迹，能循的很好！之后就很简单了，按第一题的方法从A点到B点后不用停，声光提示之后直接跑PID循迹的代码就可以，小车就会跟着圆弧走，直到走到C点！

那么如何判断小车是否已经到达了C点？

当在BC段循迹状态时，灰度传感器没有检测到任何黑线的时候，即可判断为当前已到C点（当然跟之前判断黑点一样也可能会有误差，可像之前一样增加在一段时间内好几次都没检测到黑线，也就是全白，才判断是已经到C点）。

到达C点之后要做什么？

当小车到达C点后，先产生声光提示。之前已经确定了AB方向的角度并且是沿着AB直线走的，由图可以看出来CD方向的角度正好与AD的方向相差180°，之前已经记录了AD方向的角度，因此只需要将AD方向的角度直接转180°，并将这个反向后的角度作为CD阶段的目标角度，接着按照AB直线走的那种状态通过陀螺仪角度反馈去计算当前角度与新目标角度的偏差，去调整电机差速走直线，即可从C点出来后在CD段走直线。

最后从直线到达D点时，与AB段直线走时一样去判断黑线，即可判断小车是否到达D点，如果到达了D点，随即发出声光提示并再次转入到循迹状态，小车随即会跟随DA圆弧循迹到A点，最后判断灰度传感器底下没黑线，即是到达了A点，产生声光提示并停下，第二题就完成了。

第二题总结

1. AB段按第一题的方法走即可，区别是到达B点后不用停下，产生声光提示后就转入到循迹状态去循黑线即可。

2. BC段圆弧循迹走完后到达C点检测到全白区域瞬间转入目标角度为AB反向的直线角度反馈行走即可沿CD段直走。

3. 到达D点发出声光提示后直接再次转入循迹状态无脑循黑线即可，到达A点后停下发出声光提示即完成本题。

第3题：将小车放在位置 A 点，小车能自动行驶到 C 点后，沿半弧线行驶到 B点，再由 B 点自动行驶到 D 点，最后沿半弧线行驶到 A 点停车。每经过一个点，声光提示一次。（40S）

题目三思路

本题重要的是小车启动的时候和切换状态的时候不是直线了，方向不一样了，本质上还是 直线走-识黑线-循迹走-识白面-直线走-识黑线-循迹走-识白面 ，只是当从循迹状态切换到直线状态之前需要先转一个固定的角度瞄准对面黑线后，再进行直线走。搞清楚这一点，题目就变得简单起来了，首先要学会利用陀螺仪Z轴角度反馈去控制小车的角度，编写一个角度环PID控制，有好几种方案，最简单的方案就是，同样按照之前直线走的方式（之前直线走就是用角度反馈，只不过是把对着的角度作为目标角度，现在把对着的角度的相对夹角作为目标角度了，小车启动后发现当前角度和目标角度不一样，有偏差，会自动转向调回去直到当前角度和目标角度没差），只是改变目标角度。

同样小车从A点放置正对着B点，将当前陀螺仪反馈的Z轴角度记录（也可以称初始角度校准），假设记为x°，而后同时记录AC直线走目标角度(x-38.66)°，BD直线走目标角度(x-180+38.66)°，从A点启动后直接设定目标角度为AC直线走的目标角度，此时小车会边走边转向到指定目标角度（转向偏差角度*比例系数P时P越大，差速越大，转的会更快，但太快会摆的很厉害，转太慢又没办法及时调整角度，会导致最后走到C点靠圆弧内而不是正切，可以加入D值去抑制，具体值需要测试）。小车转到目标角度后沿着设定目标角度直线走直到碰到C点，到达C点后直接转入循迹状态即可，与第二题的效果是一样的，小车将沿着CB圆弧到达B点。到达B点后，也同样转入直线走的状态，设定目标角度为BD直线走的目标角度，即可与上诉转向一样同理，小车沿着BD直线行走直到D点后，随即转入循迹状态，直到沿着圆弧到达A点，即可走完第三题的要求。记得每过一个点都要触发一次声光提示，别忘了哦。

为什么角度是38.66度？

因为题目给的地图各边长度都固定了，通过反三角函数即可计算出夹角为38.66度，由于陀螺仪的偏移与累积误差，以及小车转弯的速度和响应时间的影响，这个角度在真实情况下可能需要微调以达到实际的效果。

题目三总结

1. 与题目二大差不差，就是走直线的时候把目标角度从AB角度改成了AB角度-38.66°而已，因为第二题一开始就是走直线，不涉及转弯的过程，所以不需要怎么调参，只需要一个适当的偏差与控制量之间的比例系数即可，但本题涉及到转向，其转向的稳定程度和差速需要实际去调节，因此需要对比例系数P进行适当调节，以追求最稳定的转向和角度切入，当然最好还是使用PID控制器直接加入D值去抑制摆动，也就是常说的PID调参。

2. 本题的直线PID的调参很重要，会影响到A点和B点出点时转过去的稳定度，也会影响是否与对面的点的对齐程度。

3. 本题的循迹PID的调参也很重要，会影响到直线走到圆弧切入进去时能否正常摆入圆弧和稳定循迹。

第4题：按要求 3 的路径自动行驶 4 圈停车，用时越少越好。

本题直接在要求3的基础上加个圈数计数器，每到达A点一次计数器就加1，没跑满四圈之前到A点继续接下一次的转向直线走即可，可循环题目三的步骤即可完成第四题。

对于速度要求，直线PID的调参值，循迹PID的调参值，失误逻辑处理都息息相关，在速度快的情况下稳定度直线下降，非常考验调试与方案改进能力。

四、按键逻辑

整个按键逻辑设计如图所示，由于电赛省测的比赛现场是不允许烧代码的，为了防止换了场地或因为换地方后导致陀螺仪产生偏差，出现不稳定因素，按键加入了长按脱机调试功能，以便可以进行现场调试，如果只是学习制作小车，长按部分可以不需要写。

五、程序设计与部分细节

对于陀螺仪的数据，因为HWT101（JY61P也一样）的回报率我设置为200hz，因此每5ms陀螺仪就会向我们的单片机串口发送一次角度数据，因此我在串口中断中收到数据后解析完先放在变量里面，等待要用的时候直接去变量里面取即可即可，形成了图中的紫色的分支。

设定一个10ms的定时器中断，来执行小车PID运行的代码，小车行车代码全部都是在定时器中断里面运行的，即图中蓝色循环部分，也是定时器中断中的内容。当定时器计数值到0也就是定时器中断触发后，先判断行驶标志位变量，如果检测到标志位被置1了，则先发送陀螺仪Z轴置零指令，然后等待1.5S（注意这边等待不能用Delay延时，因为这是在中断里面，用了就卡死），陀螺仪Z轴置零完成后就保存各个直线的角度数据，以便我们后续设置目标角度。其中AB就是我们正对着的角度，直接就等于当前陀螺仪的Z轴角度，其他3条直线的角度可以根据AB的直线角度计算得出。

保存好直线角度后，就读取7路灰度传感器的电平，并对7路的状态进行偏差量化，算出当前的偏差值（也就是小车在不在黑线正中间，如果不在，那偏差了几个灯，偏差的程度是多少），如下图是我当时写的偏差量化表：

这样我们在小车启动的时候，就已经记录死了AB,CD,AC,BD四条直线的角度了，在行车过程中，也就是在定时器中断中，不断的去读取存储陀螺仪Z角度值的变量和偏差量化的变量,即可知道小车实时的当前的角度和小车底下黑白线状态。当底下全白的时候，也就是全部都是1的情况，单独设置了偏差为100，全部都是0的情况也单独设置了偏差为101，后面就可以通过判断偏差量是不是101或者102去判断底下是否是全白或者全黑。

7路灰度传感器读取与偏差量化代码参考 [grayscale.c]

​x
1
#include "grayscale.h"
2
3
bool gray_data[7]={0};//灰度数据
4
float deviations[128];//偏差索引表
5
float gray_error=0;//偏差
6
7
/*获取当前偏差值*/
8
float GrayscaleGetError(void)
9
{
10
    return gray_error;
11
}
12
/*初始化偏差值*/
13
void InitGrayscale(void)
14
{
15
    for(int i=0;i<128;i++)deviations[i]=100;//异常情况是100,值是0时为无偏差
16
    /*-循迹状态判定-*/
17
    deviations[1] = 3.0;//0000001
18
    deviations[3] = 2.5;//0000011
19
    deviations[2] = 2;//0000010
20
    deviations[6] = 1.5;//0000110
21
    deviations[4] = 1;//0000100s
22
    deviations[12] = 0.5;//0001100
23
    deviations[8] = 0;//0001000 正中间
24
    deviations[24] = -0.5;//0011000
25
    deviations[16] = -1;//0010000 
26
    deviations[48]  = -1.5;//0110000
27
    deviations[32]  = -2;//0100000
28
    deviations[96]  = -2.5;//1100000
29
    deviations[64]  = -3.0;//1000000
30
    /*特殊状态*/
31
    deviations[0]  = 101;//0000000 全背景色（全白）
32
    deviations[127]  = 102;//1111111 全线的颜色(小车拿起来也是这种状态)
33
    /*道路状况*/
34
    deviations[28] = 110;//0011100 //中间有一条横着的短线 可以是特殊标志线
35
    deviations[62] = 111;//0111110 //中间有一条横着的长线 可以是特殊标志线
36
}
37
/*读取灰度传感器电平并返回误差*/
38
float GrayscaleReadError(void)
39
{
40
    gray_data[0] = DL_GPIO_readPins(GRAY_X1_PORT,GRAY_X1_PIN)&GRAY_X1_PIN;
41
    gray_data[1] = DL_GPIO_readPins(GRAY_X2_PORT,GRAY_X2_PIN)&GRAY_X2_PIN;
42
    gray_data[2] = DL_GPIO_readPins(GRAY_X3_PORT,GRAY_X3_PIN)&GRAY_X3_PIN;
43
    gray_data[3] = DL_GPIO_readPins(GRAY_X4_PORT,GRAY_X4_PIN)&GRAY_X4_PIN;
44
    gray_data[4] = DL_GPIO_readPins(GRAY_X5_PORT,GRAY_X5_PIN)&GRAY_X5_PIN;
45
    gray_data[5] = DL_GPIO_readPins(GRAY_X6_PORT,GRAY_X6_PIN)&GRAY_X6_PIN;
46
    gray_data[6] = DL_GPIO_readPins(GRAY_X7_PORT,GRAY_X7_PIN)&GRAY_X7_PIN;
47
    //偏差量化
48
    uint32_t index = 0;
49
    for (int i = 0; i < 7; i++)index |= gray_data[i] << i;
50
    gray_error = deviations[index];
51
    return gray_error;
52
}
53
/*读取灰度传感器电平*/
54
bool* GrayscaleReadData(void)
55
{
56
    gray_data[0] = DL_GPIO_readPins(GRAY_X1_PORT,GRAY_X1_PIN)&GRAY_X1_PIN;
57
    gray_data[1] = DL_GPIO_readPins(GRAY_X2_PORT,GRAY_X2_PIN)&GRAY_X2_PIN;
58
    gray_data[2] = DL_GPIO_readPins(GRAY_X3_PORT,GRAY_X3_PIN)&GRAY_X3_PIN;
59
    gray_data[3] = DL_GPIO_readPins(GRAY_X4_PORT,GRAY_X4_PIN)&GRAY_X4_PIN;
60
    gray_data[4] = DL_GPIO_readPins(GRAY_X5_PORT,GRAY_X5_PIN)&GRAY_X5_PIN;
61
    gray_data[5] = DL_GPIO_readPins(GRAY_X6_PORT,GRAY_X6_PIN)&GRAY_X6_PIN;
62
    gray_data[6] = DL_GPIO_readPins(GRAY_X7_PORT,GRAY_X7_PIN)&GRAY_X7_PIN;
63
    return gray_data;
64
}

当走直线PID的时候，我们使用当前Z轴角度值与要走直线的绝对角度去计算偏差，然后不断调整到走直线，同时不断判断灰度传感器的偏差量化值，看看是否到了对面黑线（也就是是否要切换成循迹状态）。

当走循迹PID的时候，我们直接使用偏差量化的偏差值来作为PID控制器的误差输入，没有完整PID，只用比例系数来控制差速也可以直接用这个偏差值作为误差来调整小车的转向。

无阻塞声光提示的实现

我们正常使用SysTick中断来做一个计数器，也算系统定时器，以下是部分代码，效果就是LB_Timer变量每毫秒会减1。

xxxxxxxxxx
7
1
#define LB_Timer sysTimer[9]//声光提示延迟定时器
2
/*Systick中断(1ms触发一次)*/
3
void SysTick_Handler(void)
4
{
5
    for(int i=0;i<MAX_TIMER;i++)if(sysTimer[i])sysTimer[i]--;
6
    for(int i=0;i<MAX_TASK_TIMER;i++)if(taskTimer[i])taskTimer[i]--;
7
}

然后触发声光提示的函数：

xxxxxxxxxx
7
1
/*触发声光提示*/
2
void LB_top(void)
3
{
4
    LedSetState(1);//打开LED
5
    BeepSetState(1);//打开蜂鸣器
6
    LB_Timer=350; //设定350ms
7
}

然后在运行PID的中断主进程中，在其他代码运行之前可以先判断LB_Timer是否到0了，到0了就默认关闭LED灯和蜂鸣器。

（后面的JY61P_Z_Zero_Flag是我定义的Z轴指令是否置零完成标志位，可以不管）

xxxxxxxxxx
5
1
if((!LB_Timer) && (JY61P_Z_Zero_Flag==0))//自动关蜂鸣器和灯
2
{
3
    LedSetState(0);//关闭LED灯
4
    BeepSetState(0);//关闭蜂鸣器
5
}

这样只要调用LB_top()函数，会打开LED灯与蜂鸣器，350ms后PID中断进程判断到了LB_Timer变量变为0了，就自动把LED灯与蜂鸣器关闭了，实现了无阻塞的延迟。

陀螺仪的角度偏差的正确计算

实测无论是JY61P还是HTW101,往左转角度都是增加，往右转角度都是减少，增加到180°会变成0°，减少到0°会变成-180°，那么如何去根据这种变化去正确计算当前角度与目标角度的差值呢？我研究了一套可用的角度计算算法可以正确求出偏差值，其中floor是C语言中的向下取整函数，在头文件<math.h>里面。

xxxxxxxxxx
2
1
float one_error = 目标角度 - 当前角度;
2
float 偏差角度 = (one_error + 180) - floor((one_error + 180) / 360) * 360 - 180;

离散数字量位置式PID运算

PID算法表达式：

计算机控制是一种采样控制，它只能根据采样时刻的偏差来计算控制量，并且由于我们的灰度传感器和陀螺仪传输的数据也不是连续的，而是每隔一段固定时间接收一次，并且我们也需要每隔一个固定时间（定时器中断PID间隔时间）才能计算一次，所以必须对上述公式进行离散化处理，最终得到我们的离散化PID公式：

其中T=Δt为采样周期，必须使T足够小，才能保证系统有一定精度，因此用于运算PID的定时器中断的时间间隔最好是50ms以下。

PID控制器结构体定义：

xxxxxxxxxx
10
1
typedef struct {
2
    float kp;//比例增益             (响应)
3
    float ki;//积分增益             (消除稳态误差)
4
    float kd;//微分增益             (阻尼)
5
    float last_err;//上一次的误差   (作用d)
6
    float err_sum;//误差累加        (积分值:作用i)
7
    float err_sum_limit;//积分限幅  (防止超出:作用i)
8
    float target;//目标值           (要达到的目标值)
9
    float output_limit;//输出限幅   (防止超出控制量范围)
10
}PIDController;

PID控制器初始化函数：

xxxxxxxxxx
12
1
/*初始化PID控制器 (控制器指针,kp,ki,kd,目标值,积分限幅,输出限幅) */
2
void PID_Init(PIDController *pid,float kp,float ki,float kd,float target,float err_sum_limit,float output_limit)
3
{
4
    pid->kp=kp;
5
    pid->ki=ki;
6
    pid->kd=kd;
7
    pid->last_err=0;
8
    pid->err_sum=0;
9
    pid->err_sum_limit=err_sum_limit;
10
    pid->target=target;
11
    pid->output_limit=output_limit;
12
}

PID控制器初始化调用：

xxxxxxxxxx
19
1
#define StraightSpeed 260//直线走速度
2
#define LineSpeed 260//循迹速度
3
4
float const_z_angle_r=0.0f;//默认右绝对角度z(AB直线)
5
float const_z_angle_l=180.0f;//默认左绝对角度-z(BA直线)
6
7
float const_z_offset_right=38.35f;//右斜线角度偏移 38.66
8
float const_z_offset_left=39.1f;//左斜线角度偏移 38.66
9
10
float straight_kp=9.0f,straight_ki=0,straight_kd=90;//直线PID参数
11
float line_kp=55.0f,line_ki=0,line_kd=170000.0f;//循迹PID参数
12
/*初始化PID*/
13
void InitTracking(void)
14
{
15
    NVIC_EnableIRQ(TIMER_RUN_INST_INT_IRQN);//启用PID运算定时器中断
16
    NVIC_ClearPendingIRQ(TIMER_RUN_INST_INT_IRQN);//清除中断标志位
17
    PID_Init(&straight_pid, straight_kp, straight_ki, straight_kd, const_z_angle_r, 100, StraightSpeed);//初始化直线PID控制器
18
    PID_Init(&line_pid, line_kp, line_ki, line_kd, 0, 300, LineSpeed);//初始化循迹PID控制器
19
}

直线（转向）PID运算函数（参数current_angle为当前陀螺仪获取的角度，角度环只用到了PD参数）：

xxxxxxxxxx
18
1
/*直走PID运算*/
2
float PID_Calculate_Straight(PIDController *pid,float current_angle)
3
{
4
    //陀螺仪的角度偏差的正确计算
5
    float one_error = pid->target - current_angle;
6
    float adjustedError = (one_error + 180) - floor((one_error + 180) / 360) * 360 - 180;
7
    float err = adjustedError;
8
    //开始计算
9
    pid->err_sum+=err;//误差累加
10
    if((pid->err_sum)>(pid->err_sum_limit))pid->err_sum=(pid->err_sum_limit);//正积分i限幅
11
    if((pid->err_sum)<(-pid->err_sum_limit))pid->err_sum=-(pid->err_sum_limit);//负积分i限幅
12
    float err_difference = err - pid->last_err;//误差变化量
13
    pid->last_err=err;//保存误差
14
    float output = pid->kp*err + pid->ki*pid->err_sum + pid->kd*err_difference;//运算
15
    if(output>(pid->output_limit))output=(pid->output_limit);//正输出限幅
16
    if(output<-(pid->output_limit))output=-(pid->output_limit);//负输出限幅
17
    return output;//返回运算结果
18
}

循迹PID运算函数（参数current_angle为当前灰度传感器偏差量化的偏差值，循迹只需要用到PD参数）：

xxxxxxxxxx
14
1
/*循迹PID运算*/
2
float PID_Calculate_Line(PIDController *pid,float current_error)
3
{
4
    float err=pid->target - current_error;
5
    pid->err_sum+=err;//误差累加
6
    if((pid->err_sum)>(pid->err_sum_limit))pid->err_sum=(pid->err_sum_limit);//正积分i限幅
7
    if((pid->err_sum)<(-pid->err_sum_limit))pid->err_sum=-(pid->err_sum_limit);//负积分i限幅
8
    float err_difference = err - pid->last_err;//误差变化量
9
    pid->last_err=err;//保存误差
10
    float output = pid->kp*err + pid->ki*pid->err_sum + pid->kd*err_difference;//运算
11
    if(output>(pid->output_limit))output=(pid->output_limit);//正输出限幅
12
    if(output<-(pid->output_limit))output=-(pid->output_limit);//负输出限幅
13
    return output;//返回运算结果
14
}

PID代码调用具体实现代码示例

直线PID控制电机

xxxxxxxxxx
14
1
/*小车运行PID计算定时器中断函数 10ms-50ms都可以*/
2
void TIMER_RUN_INST_IRQHandler(void)
3
{
4
    switch (DL_TimerG_getPendingInterrupt(TIMER_RUN_INST))
5
    {
6
        case DL_TIMER_IIDX_ZERO:
7
        {
8
                float mea_ang = JY61P_GetAngle()[2];//获取Z轴角度
9
                float pwm_out = PID_Calculate_Straight(&straight_pid, mea_ang);//PID计算
10
                MotorSetPwmDuty(0, StraightSpeed-pwm_out);//根据输出量调整PWM占空比
11
                MotorSetPwmDuty(1, StraightSpeed+pwm_out);//根据输出量调整PWM占空比
12
        }
13
    }
14
}

循迹PID控制电机

由于灰度传感器的偏差量化值设计的是浮点型，在编程语言中小数不能直接比较，为此专门写一个小数比较函数来使用。

xxxxxxxxxx
9
1
/*判断两个小数是否相等*/
2
bool float_equal(float a,float b)
3
{
4
    float epsilon = 0.00001f;//定义容忍度epsilon
5
    if (fabs(a - b) < epsilon)//使用fabs函数计算a和b之间的绝对差，判断是否小于epsilon
6
        return true;
7
    else
8
        return false;
9
}

循迹PID运算：

xxxxxxxxxx
29
1
float last_gray_error=0;//上次的循迹的偏差值
2
/*小车运行PID计算定时器中断函数 10ms-50ms都可以*/
3
void TIMER_RUN_INST_IRQHandler(void)
4
{
5
    switch (DL_TimerG_getPendingInterrupt(TIMER_RUN_INST))
6
    {
7
        case DL_TIMER_IIDX_ZERO:
8
        {
9
                float gray_error = GrayscaleReadError();//获取灰度传感器偏差
10
                if(float_equal(gray_error,101.0f))//全白的话按之前的偏差来行进
11
             {
12
                int32_t pwm_out = PID_Calculate_Line(&line_pid, last_gray_error);
13
                MotorSetPwmDuty(0, LineSpeed+pwm_out);
14
                MotorSetPwmDuty(1, LineSpeed-pwm_out);
15
             }else if(float_equal(gray_error,100.0f))//异常状态也按之前的来行进
16
             {
17
                int32_t pwm_out = PID_Calculate_Line(&line_pid, last_gray_error);
18
                MotorSetPwmDuty(0, LineSpeed+pwm_out);
19
                MotorSetPwmDuty(1, LineSpeed-pwm_out);
20
             }else
21
             {//正常巡线
22
                int32_t pwm_out = PID_Calculate_Line(&line_pid, gray_error);
23
                MotorSetPwmDuty(0, LineSpeed+pwm_out);
24
                MotorSetPwmDuty(1, LineSpeed-pwm_out);
25
                last_gray_error = gray_error;//更新上次循迹偏差值
26
             }
27
        }
28
    }
29
}

如果要改变PID控制器内的PID参数，请在前面的PID控制器初始化中那个传入的PID值和目标值那边去修改，当然也可以单独写一个函数把修改PID控制器参数封装起来，如果要实时修改就可以调用这个函数，方便调节PID参数。

xxxxxxxxxx
24
1
/*改变P参数*/
2
void PID_Change_kp(PIDController *pid,float kp)
3
{
4
    pid->kp=kp;
5
}
6
/*改变I参数*/
7
void PID_Change_ki(PIDController *pid,float ki)
8
{
9
    pid->ki=ki;
10
    pid->err_sum=0;
11
}
12
/*改变D参数*/
13
void PID_Change_kd(PIDController *pid,float kd)
14
{
15
    pid->kd=kd;
16
    pid->last_err=0;
17
}
18
/*改变目标值(误差要重置)*/
19
void PID_Change_target(PIDController *pid,float target)
20
{
21
    pid->target=target;
22
    pid->last_err=0;
23
    pid->err_sum=0;
24
}

结束

tips：关于

版本：V1.0.0

作者：21 物联 陈东豪

V1.0.0 ：2024 年 10 月 16 日