从接触编码器到调出好康的PID波形

- 认识电机及编码器
- 只需动动手指，就能让STM32得到电机转过的角度
- 让电机转起来
- 认识PID控制
- - ①比例调节器
  - ②积分调节
  - ③微分调节
  - ④比例积分微分调节
  - 数字PID调节器
  - - （1）数字PID位置型控制算法
    - （2）数字PID增量型控制算法
- PID控制器代码
- 在程序中加入PID控制
- - 凑试法整定PID参数
  - 两个改进PID控制器的技巧
  - - 积分分离
    - 加入死区
最终的带积分分离及死区的改进型PID算法
- PID.c
- PID.h
修正
- Encoder.c
- Encoder.h

本文所用编程环境：STM32 Cube IDE 1.5.0

认识电机及编码器

在这里插入图片描述
这种灰头土脸的直流减速电机应该是各大科创比赛上最常见的了。
它们采用的是增量式编码器，价格低廉。

不论是光电编码器还是霍尔编码器，都会产生一对正交的脉冲信号来间接告诉我们电机状况。
在这里插入图片描述

只要按照一定规则对这对正交信号解读，就能得到我们所需的信息。
幸运的是，许多微控制器都带有硬件解码电路，大大减弱了我们上手的难度。

两个正交编码脉冲输入信号的两个边沿均被正交编码脉冲电路计数，因此由其产生的时钟频率是每个输入序列频率的4倍，这个时钟将作为计数器的时钟源信号。
在这里插入图片描述

只需动动手指，就能让STM32得到电机转过的角度

首先选择你所用的芯片，本文所用的开发板是野火的指南者，STM32F103VET6
在这里插入图片描述
输入工程名称后，将高速时钟的时钟源设为外部晶振。

别忘了将debug选项与你的调试方法适配，如果用的是普通的U盘状ST-Link V2，选Serial Wire就行。

在时钟树中，将系统时钟源锁相环倍频后的时钟信号，并将HCLK设为一个合理的值，按下回车，软件会自动计算各部分所需的分频系数、预定标系数。
在这里插入图片描述
根据开发板引脚情况激活一个带有编码器模式的定时器，设定其装载值、使能自动重装载，并将编码器模式设为双通道模式，以实现4倍分辨率的提升。本文所用的编码器线数为13ppr，故当装载值设为13-1时可在溢出中断时得到整数的角度分化。
在这里插入图片描述

使能所用定时器的中断。

点击左上角的齿轮符号生成配置好的工程文件。
在这里插入图片描述
在主函数中加入几行代码，以开始编码器工作模式。

  /* Initialize all configured peripherals */MX_GPIO_Init();MX_TIM3_Init();/* USER CODE BEGIN 2 */HAL_TIM_Encoder_Start(&htim3,TIM_CHANNEL_ALL);	//编码器模式启动，写ALL为开始该定时器（TIMx）的通道1和通道2（编码器模式可自动计算）。使用编码器模式不用输入捕获HAL_TIM_Encoder_Start_IT(&htim3,TIM_CHANNEL_ALL);	//开启中断__HAL_TIM_ENABLE_IT(&htim3,TIM_IT_UPDATE);   		//使能更新中断htim3.Instance->CNT = 0;/* USER CODE END 2 *//* Infinite loop *//* USER CODE BEGIN WHILE */while (1){/* USER CODE END WHILE *//* USER CODE BEGIN 3 */}/* USER CODE END 3 */
}

暂时只需将编码器电路的5V、GND、AB相接好。本文将A相接入PA6，B相接入PA7。
在这里插入图片描述
把调试器接上后点击debug和继续图标开始调试。

在现场表达式窗口中，监看htim3.Instance->CNT的计数值，轻轻拨动码盘，
在这里插入图片描述
可以发现，码盘每正向转动约1/4圈，计数值将从0至12循环变化。

接下来就是将计数值转化为主轴转过的角度了
加入下列程序
其中void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)是弱定义修饰的函数，需要我们给出其实现。

/* USER CODE BEGIN 4 */int Angle = 0;
int Target_Angle = 180;
const int Step_Angle = 360/4/30;	//每两次计数器溢出中断时，主轴转过的角度�??
void Motor_Get_Angle(TIM_HandleTypeDef *htim)	//放到HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)中
{if(htim == &htim3){if((htim->Instance->CR1 & 0x0010)>>4)	//查询相应控制寄存器的第4位以判断转向Angle -= Step_Angle;elseAngle += Step_Angle;}
}/*** @brief  Period elapsed callback in non-blocking mode* @param  htim TIM handle* @retval None*/
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{Motor_Get_Angle(htim);/* Prevent unused argument(s) compilation warning */UNUSED(htim);/* NOTE : This function should not be modified, when the callback is needed,the HAL_TIM_PeriodElapsedCallback could be implemented in the user file*/
}/* USER CODE END 4 */

本文中所用的电机减速比为30，电机机身上也写着30F，故码盘每转动30圈，主轴转动1圈。
在这里插入图片描述

又由于我们采用正交编码电路解码，分辨率提高了4倍，因此每两次同向溢出间的步进角度
Step_Angle = 360/4/30 = 3°
通过查阅手册得知，我们可以通过查询相应控制寄存器的第4位以判断转向。
在这里插入图片描述

再次进入debug，将Angle加入监看，转动主轴，可以发现主轴的角度成功地解读出来了。

使能串口1

加入下列代码重定向以使用printf函数。

/* Private variables ---------------------------------------------------------*/
TIM_HandleTypeDef htim3;UART_HandleTypeDef huart1;/* USER CODE BEGIN PV */
#include <stdio.h>#ifdef __GNUC__
#define PUTCHAR_PROTOTYPE int __io_putchar(int ch)
#else
#define PUTCHAR_PROTOTYPE int fputc(int ch, FILE *f)
#endif
PUTCHAR_PROTOTYPE
{HAL_UART_Transmit(&huart1, (uint8_t *)&ch, 1, 0xFFFF);return ch;
}
/* USER CODE END PV *//* Private function prototypes -----------------------------------------------*/

在main的while循环中加入下列代码，就可以通过串口在上位机上显示角度。

  /* Infinite loop *//* USER CODE BEGIN WHILE */while (1){extern int Angle;printf("%d\r\n", Angle);HAL_Delay(10);/* USER CODE END WHILE *//* USER CODE BEGIN 3 */}/* USER CODE END 3 */

本文所用的上位机是VOFA+，绘图很方便。
在这里插入图片描述
在控件页面中拖入一个波形图控件

在协议与连接模块中，配置串口信息，点击左上角的小圆点连接，之后在波形图中将数据加入y轴。

转转主轴，按下右下角的Auto键，拖动下方的滚动条设置窗口，就能得到编码器返回的波形
在这里插入图片描述

让电机转起来

单片机的IO口一般所能输出的是0~3.3/5V的数字离散信号。
在这里插入图片描述
因此如果我们人为地控制输出信号中的高电平和低电平比例，就能产生平均电压可控的控制信号。
文本所用的驱动模块是经典的L298N，当输入的两路控制信号不同时，输出也不同。当输入为TTL高电平，对应的输出便是高驱动能力的工作电压，可以将L298N视为一个电平转换器，将0~3.3/5V的TTL信号转变为0—12V的驱动信号。
若输入1，0的控制信号，则输出的两路间的电压差为12V，反之输入0，1的控制信号，输出的两路间的电压差就为-12V，从而实现控制电机的正反转。
在这里插入图片描述
根据开发板选择定时器产生PWM波形：设置定时器时钟源为内部时钟，使能要用的通道，设置预分频系数和重装载值并使能自动重装载。

在这里插入图片描述
由于定时器时钟均为72MHz，故当预分频系数设为72-1，重装载值设为1000-1，就可得到72M/72/1000=1000Hz的PWM信号，分辨率为1000。

本文中，将PC6、PC7接入L298N的输入端，输出端接入电机线+、电机线-，将开发板与L298N共地。
在主循环前加入下列代码，就能产生占空比为50%的控制信号令电机转动。

  HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_1);HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_2);__HAL_TIM_SetCompare(&htim8, TIM_CHANNEL_1, 500);__HAL_TIM_SetCompare(&htim8, TIM_CHANNEL_2, 0);

认识PID控制

PID控制是一种负反馈控制
PID调节器是一种线性调节器，这种调节器是将设定值r和实际输出值y进行比较，构成控制偏差：e=r-y，并将其比例、积分和微分通过线性组合构成控制量。如图：
在这里插入图片描述
在实际应用中，根据对象的特性和控制要求，也可灵活地改变其结构，取其中一部分环节构成控制规律，例如P，PI，PD等。

①比例调节器

最简单的一种调节器
控制规律：
在这里插入图片描述

其中，Kp为比例系数，U0是控制量的基准，也就是当误差e(t)=0时的控制作用（比如阀门的起始开度、基准的信号等
特点：有差调节，只要偏差出现，就能及时地产生与之成比例的调节作用，具有调节及时的特点。
偏差e的大小，受比例系数的影响。
比例作用：迅速反应误差，加大比例系数，可以减小静差，但不能消除稳态误差，过大容易引起不稳定。
在这里插入图片描述
阶跃响应特性曲线

②积分调节

所谓积分作用是指调节器的输出与输入偏差的积分成比例的作用
控制规律：
在这里插入图片描述
其中，S0为积分速度。
特点：①无差调节；
　　　②稳定性变差：积分引入了-90度相角。
积分作用：消除静差，积分作用太强容易引起超调，甚至出现振荡。

积分作用响应曲线

③微分调节

在这里插入图片描述
微分作用：减小超调，克服振荡，提高稳定性，改善系统动态特性。

微分作用响应曲线

④比例积分微分调节

在这里插入图片描述
比例控制能迅速反应误差，偏差一旦产生，控制器立即产生控制作用，从而减小误差，但比例控制不能消除稳态误差，KP的加大，会引起系统的不稳定；
积分控制主要用于消除静差，提高系统的无差度。只要系统存在误差，积分控制作用就不断地积累，输出控制量以消除误差，因而，只要有足够的时间，积分控制将能完全消除误差，积分作用太强会使系统超调加大，甚至使系统出现振荡；
微分环节能反映偏差信号的变化趋势，并能在偏差信号值变得太大之前，在系统中引入一个有效的早期修正信号，加快系统的动态响应速度，减小调整时间，同时可以减小超调量，克服振荡，使系统的稳定性提高从而改善系统的动态性能。

优点

技术成熟。 P、I、D三个参数的优化配置，兼顾了动态过程的现在、过去与将来的信息，使动态过程快速、平稳和准确
算法简单，易被人们熟悉和掌握
不需要建立对象数学模型
控制效果好
适应性好，鲁棒性强

数字PID调节器

用数值逼近的方法实现PID控制规律。
数值逼近的方法：当采样周期相当短时，用求和代替积分、用后向差分代替微分，使模拟PID离散化为差分方程。

（1）数字PID位置型控制算法

在这里插入图片描述
可得：

位置型控制算法提供执行机构的位置u(k)，比如阀门的开度。

（2）数字PID增量型控制算法

根据位置型控制算法写出u(k-1)：
在这里插入图片描述
u(k)- u(k-1)可得：

在这里插入图片描述
增量型控制算法提供执行机构的增量△ u(k)，比如步进电机的步数。
在控制系统中：
①如执行机构采用调节阀，则控制量对应阀门的开度，表征了执行机构的位置，此时控制器应采用数字PID位置式控制算法；
②如执行机构采用步进电机，每个采样周期，控制器输出的控制量，是相对于上次控制量的增加，此时控制器应采用数字PID增量式控制算法；
在这里插入图片描述
增量式控制算法的优点：
(1)增量算法不需要做累加，控制量增量的确定仅与最近几次误差采样值有关，计算误差或计算精度问题，对控制量的计算影响较小。而位置算法要用到过去的误差的累加值，容易产生大的累加误差。
(2)增量式算法得出的是控制量的增量，例如阀门控制中只输出阀门开度的变化部分，误动作影响小，必要时通过逻辑判断限制或禁止本次输出，不会严重影响系统的工作。而位置算法的输出是控制量的全量输出，误动作影响大。
(3)采用增量算法，易于实现手动到自动的无冲击切换。
增量式PID控制算法与位置式PID控制算法相比，有下列缺点：
(1) 积分截断效应大，有静态误差；
(2) 溢出的影响大。

PID控制器代码

取自大疆Robomaster 开发板C例程。
源文件：

/******************************(C) COPYRIGHT 2019 DJI***************************** @file       pid.c/h* @brief      pid实现函数，包括初始化，PID计算函数，* @note* @history*  Version    Date            Author          Modification*  V1.0.0     Dec-26-2018     RM              1. 完成*@verbatim============================================================================================================================================================@endverbatim****************************(C) COPYRIGHT 2019 DJI*****************************/#include "../PID/PID.h"#include "main.h"#define LimitMax(input, max)   \{                          \if (input > max)       \{                      \input = max;       \}                      \else if (input < -max) \{                      \input = -max;      \}                      \}/*** @brief          pid struct data init* @param[out]     pid: PID struct data point* @param[in]      mode: PID_POSITION: normal pid*                 PID_DELTA: delta pid* @param[in]      PID: 0: kp, 1: ki, 2:kd* @param[in]      max_out: pid max out* @param[in]      max_iout: pid max iout* @retval         none*/
/*** @brief          pid struct data init* @param[out]     pid: PID结构数据指针* @param[in]      mode: PID_POSITION:普通PID*                 PID_DELTA: 差分PID* @param[in]      PID: 0: kp, 1: ki, 2:kd* @param[in]      max_out: pid最大输出* @param[in]      max_iout: pid最大积分输出* @retval         none*/
void PID_init(pid_type_def *pid, uint8_t mode, const fp32 PID[3], fp32 max_out, fp32 max_iout)
{if (pid == NULL || PID == NULL){return;}pid->mode = mode;pid->Kp = PID[0];pid->Ki = PID[1];pid->Kd = PID[2];pid->max_out = max_out;pid->max_iout = max_iout;pid->Dbuf[0] = pid->Dbuf[1] = pid->Dbuf[2] = 0.0f;pid->error[0] = pid->error[1] = pid->error[2] = pid->Pout = pid->Iout = pid->Dout = pid->out = 0.0f;
}/*** @brief          pid calculate* @param[out]     pid: PID struct data point* @param[in]      ref: feedback data* @param[in]      set: set point* @retval         pid out*/
/*** @brief          pid计算* @param[out]     pid: PID结构数据指针* @param[in]      ref: 反馈数据* @param[in]      set: 设定值* @retval         pid输出*/
fp32 PID_calc(pid_type_def *pid, fp32 ref, fp32 set)
{if (pid == NULL){return 0.0f;}pid->error[2] = pid->error[1];pid->error[1] = pid->error[0];pid->set = set;pid->fdb = ref;pid->error[0] = set - ref;if (pid->mode == PID_POSITION){pid->Pout = pid->Kp * pid->error[0];pid->Iout += pid->Ki * pid->error[0];pid->Dbuf[2] = pid->Dbuf[1];pid->Dbuf[1] = pid->Dbuf[0];pid->Dbuf[0] = (pid->error[0] - pid->error[1]);pid->Dout = pid->Kd * pid->Dbuf[0];LimitMax(pid->Iout, pid->max_iout);pid->out = pid->Pout + pid->Iout + pid->Dout;LimitMax(pid->out, pid->max_out);}else if (pid->mode == PID_DELTA){pid->Pout = pid->Kp * (pid->error[0] - pid->error[1]);pid->Iout = pid->Ki * pid->error[0];pid->Dbuf[2] = pid->Dbuf[1];pid->Dbuf[1] = pid->Dbuf[0];pid->Dbuf[0] = (pid->error[0] - 2.0f * pid->error[1] + pid->error[2]);pid->Dout = pid->Kd * pid->Dbuf[0];pid->out += pid->Pout + pid->Iout + pid->Dout;LimitMax(pid->out, pid->max_out);}return pid->out;
}/*** @brief          pid out clear* @param[out]     pid: PID struct data point* @retval         none*/
/*** @brief          pid 输出清除* @param[out]     pid: PID结构数据指针* @retval         none*/
void PID_clear(pid_type_def *pid)
{if (pid == NULL){return;}pid->error[0] = pid->error[1] = pid->error[2] = 0.0f;pid->Dbuf[0] = pid->Dbuf[1] = pid->Dbuf[2] = 0.0f;pid->out = pid->Pout = pid->Iout = pid->Dout = 0.0f;pid->fdb = pid->set = 0.0f;
}

头文件：

/******************************(C) COPYRIGHT 2016 DJI***************************** @file       pid.c/h* @brief      pid实现函数，包括初始化，PID计算函数，* @note* @history*  Version    Date            Author          Modification*  V1.0.0     Dec-26-2018     RM              1. 完成*@verbatim============================================================================================================================================================@endverbatim****************************(C) COPYRIGHT 2016 DJI*****************************/
#ifndef PID_H
#define PID_H#include "main.h"typedef float fp32;
typedef double fp64;enum PID_MODE
{PID_POSITION = 0,PID_DELTA
};typedef struct
{uint8_t mode;//PID 三参数fp32 Kp;fp32 Ki;fp32 Kd;fp32 max_out;  //最大输出fp32 max_iout; //最大积分输出fp32 set;fp32 fdb;fp32 out;fp32 Pout;fp32 Iout;fp32 Dout;fp32 Dbuf[3];  //微分项 0最新 1上一次 2上上次fp32 error[3]; //误差项 0最新 1上一次 2上上次} pid_type_def;
/*** @brief          pid struct data init* @param[out]     pid: PID struct data point* @param[in]      mode: PID_POSITION: normal pid*                 PID_DELTA: delta pid* @param[in]      PID: 0: kp, 1: ki, 2:kd* @param[in]      max_out: pid max out* @param[in]      max_iout: pid max iout* @retval         none*/
/*** @brief          pid struct data init* @param[out]     pid: PID结构数据指针* @param[in]      mode: PID_POSITION:普通PID*                 PID_DELTA: 差分PID* @param[in]      PID: 0: kp, 1: ki, 2:kd* @param[in]      max_out: pid最大输出* @param[in]      max_iout: pid最大积分输出* @retval         none*/
extern void PID_init(pid_type_def *pid, uint8_t mode, const fp32 PID[3], fp32 max_out, fp32 max_iout);/*** @brief          pid calculate* @param[out]     pid: PID struct data point* @param[in]      ref: feedback data* @param[in]      set: set point* @retval         pid out*/
/*** @brief          pid计算* @param[out]     pid: PID结构数据指针* @param[in]      ref: 反馈数据* @param[in]      set: 设定值* @retval         pid输出*/
extern fp32 PID_calc(pid_type_def *pid, fp32 ref, fp32 set);/*** @brief          pid out clear* @param[out]     pid: PID struct data point* @retval         none*/
/*** @brief          pid 输出清除* @param[out]     pid: PID结构数据指针* @retval         none*/
extern void PID_clear(pid_type_def *pid);#endif

PID_Init 函数的功能为设置 PID 控制器的模式，各项系数，最大输出值和积分上限，最后将
PID 控制器的输入和输出均初始化为 0。
而计算 PID 使用的 PID_calc 函数则首先区分 PID 的模式是位置式还是增量式，位置式则使
用公式
在这里插入图片描述
增量式则使用公式：

在程序中加入PID控制

/* USER CODE BEGIN Includes */
#include "../PID/PID.h"
#include <math.h>#define Motor_KP	0
#define Motor_KI	0
#define Motor_KD	0pid_type_def Motor_PID;
const static fp32 motor_pid[3]	= {Motor_KP, Motor_KI, Motor_KD};
/* USER CODE END Includes */

  HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_1);HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_2);PID_init(&Motor_PID, PID_POSITION, motor_pid, 1000, 1000);/* USER CODE END 2 *//* Infinite loop *//* USER CODE BEGIN WHILE */while (1){extern int Angle;extern int Target_Angle;PID_calc(&Motor_PID, Angle, Target_Angle);if(Motor_PID.out > 0){__HAL_TIM_SetCompare(&htim8, TIM_CHANNEL_1, Motor_PID.out);	__HAL_TIM_SetCompare(&htim8, TIM_CHANNEL_2, 0);}else{__HAL_TIM_SetCompare(&htim8, TIM_CHANNEL_1, 0);__HAL_TIM_SetCompare(&htim8, TIM_CHANNEL_2, -Motor_PID.out);}printf("%d,%d\r\n", Angle, Target_Angle);HAL_Delay(10);/* USER CODE END WHILE *//* USER CODE BEGIN 3 */}

进入debug模式监看Target_Angle及Motor_PID结构体
在这里插入图片描述
回到VOFA+中，将Target_Angle加入波形图

凑试法整定PID参数

在这里插入图片描述
根据Kp,i,d对控制过程的影响趋势，参数整定采用先比例，后积分，再微分的整定步骤。
①首先整定比例部分，系数由小变大，得到反应快，超调小的响应曲线。如果系统已满足静差要求，则直接使用比例即可。
比例项 Kp 过小时，PID 控制器的反应速度较慢且存在静差。静差是指控制器的最终输出保
持为一个和期望值存在一定误差的值，引发静差的原因时由于比例控制的输出和误差成线性
关系，如果当误差值减小时，比例控制器的输出值同样会减少，导致比例控制器不可能达到
和期望值完全相同，即误差值为零的情况，因为如果误差值为零则比例控制器的输出也会为
零
在这里插入图片描述
直接在监看表达式中修改Kp的值

慢慢增大Kp并修改Target_Angle以观察响应曲线。

②比例调节系统静差不能满足要求时加入积分环节。所要达到的目的：消除静差效果好。

在这里插入图片描述

③经上两步后，动态过程仍不能令人满意时，可加d环节，d由小到大变化。先取d为零，逐步增大d，同时改变比例参数和积分时间，直到系统得到好的动态性能和效果。
在这里插入图片描述

在这里插入图片描述
我们发现，加入微分环节后，超调量得到了一定程度的抑制，控制但效果仍不理想，这是积分环节造成的。

两个改进PID控制器的技巧

积分控制主要用于消除静差，提高系统的无差度。积分作用太强会使系统超调加大，甚至使系统出现振荡。

积分分离

－改进原因：在过程的启动、结束或大幅度增减设定值时，短时间内系统输出有很大的偏差，会造成PID运算的积分积累。由于系统的惯性和滞后，在积分累积项的作用下，往往会产生较大的超调和长时间的波动。特别对于温度、成份等变化缓慢的过程，这一现象更为严重。
－改进思路：当被控量和给定值偏差大时，取消积分控制，以免超调量过大；当被控量和给定值接近时，积分控制投入，消除静差。
－改进方法：
当 |e(k)|> β时，采用PD控制；
当 |e(k)|< β时，采用PID控制。
对于积分分离，应该根据具体对象及控制要求合理的选择阈值β
积分分离阈值β的确定：
β过大，达不到积分分离的目的； β过小，则一旦被控量y(t)无法跳出各积分分离区，只进行PD控制，将会出现残差。
在这里插入图片描述

  while (1){extern int Angle;extern int Target_Angle;if(fabs(Target_Angle-Angle)>15){Motor_PID.Ki = 0;Motor_PID.Iout = 0;}elseMotor_PID.Ki = Motor_KI;PID_calc(&Motor_PID, Angle, Target_Angle);if(Motor_PID.out > 0){__HAL_TIM_SetCompare(&htim8, TIM_CHANNEL_1, Motor_PID.out);__HAL_TIM_SetCompare(&htim8, TIM_CHANNEL_2, 0);}else{__HAL_TIM_SetCompare(&htim8, TIM_CHANNEL_1, 0);__HAL_TIM_SetCompare(&htim8, TIM_CHANNEL_2, -Motor_PID.out);}printf("%d,%d\r\n", Angle, Target_Angle);HAL_Delay(10);/* USER CODE END WHILE *//* USER CODE BEGIN 3 */}

加入积分分离后再调节，就很容易得到控制效果不错的波形了
在这里插入图片描述

加入死区

－改进原因：在精度不高的场合，为了避免控制动作过于频繁，以消除由于频繁动作所引起的振荡和能量消耗，有时采用带有死区的PID控制系统。
在这里插入图片描述

由于本实验中所测电机角度分辨率为3°，为避免当误差较小时频繁动作，加入死区。

  /* Infinite loop *//* USER CODE BEGIN WHILE */while (1){extern int Angle;extern int Target_Angle;if(fabs(Target_Angle-Angle)>15){Motor_PID.Ki = 0;Motor_PID.Iout = 0;PID_calc(&Motor_PID, Angle, Target_Angle);}else if(fabs(Target_Angle-Angle)<3){PID_calc(&Motor_PID, Angle, Angle);}else{Motor_PID.Ki = Motor_KI;PID_calc(&Motor_PID, Angle, Target_Angle);}if(Motor_PID.out > 0){__HAL_TIM_SetCompare(&htim8, TIM_CHANNEL_1, Motor_PID.out);__HAL_TIM_SetCompare(&htim8, TIM_CHANNEL_2, 0);}else{__HAL_TIM_SetCompare(&htim8, TIM_CHANNEL_1, 0);__HAL_TIM_SetCompare(&htim8, TIM_CHANNEL_2, -Motor_PID.out);}printf("%d,%d\r\n", Angle, Target_Angle);HAL_Delay(10);/* USER CODE END WHILE *//* USER CODE BEGIN 3 */}/* USER CODE END 3 */

别忘了把调好的参数写回去

#define Motor_KP	15
#define Motor_KI	0.2
#define Motor_KD	75

最终的带积分分离及死区的改进型PID算法

PID.c

/******************************(C) COPYRIGHT 2019 DJI***************************** @file       pid.c/h* @brief      pid实现函数，包括初始化，PID计算函数，* @note* @history*  Version    Date            Author          Modification*  V1.0.0     Dec-26-2018     RM              1. 完成*@verbatim============================================================================================================================================================@endverbatim****************************(C) COPYRIGHT 2019 DJI*****************************/#include "PID.h"
#include "math.h"#define NULL 0#define LimitMax(input, max)   \{                          \if (input > max)       \{                      \input = max;       \}                      \else if (input < -max) \{                      \input = -max;      \}                      \}/*** @brief          pid struct data init* @param[out]     pid: PID struct data point* @param[in]      mode: PID_POSITION: normal pid*                 PID_DELTA: delta pid* @param[in]      PID: 0: kp, 1: ki, 2:kd* @param[in]      max_out: pid max out* @param[in]      max_iout: pid max iout* @retval         none*/
/*** @brief          pid struct data init* @param[out]     pid: PID结构数据指针* @param[in]      mode: PID_POSITION:普通PID*                 PID_DELTA: 差分PID* @param[in]      PID: 0: kp, 1: ki, 2:kd* @param[in]      max_out: pid最大输出* @param[in]      max_iout: pid最大积分输出* @retval         none*/
void PID_init(pid_type_def *pid, uint8_t mode, const fp32 PID[3], fp32 max_out, fp32 max_iout, fp32 Integral_Separation_Threshold, fp32 Dead_Zone)
{if (pid == NULL || PID == NULL){return;}pid->mode = mode;pid->Kp = PID[0];pid->Ki = PID[1];pid->Kd = PID[2];pid->max_out = max_out;pid->max_iout = max_iout;pid->Dbuf[0] = pid->Dbuf[1] = pid->Dbuf[2] = 0.0f;pid->error[0] = pid->error[1] = pid->error[2] = pid->Pout = pid->Iout = pid->Dout = pid->out = 0.0f;pid->Integral_Separation_Threshold = Integral_Separation_Threshold;pid->Dead_Zone = Dead_Zone;
}/*** @brief          pid calculate* @param[out]     pid: PID struct data point* @param[in]      ref: feedback data* @param[in]      set: set point* @retval         pid out*/
/*** @brief          pid计算* @param[out]     pid: PID结构数据指针* @param[in]      ref: 反馈数据* @param[in]      set: 设定值* @retval         pid输出*/
fp32 PID_calc(pid_type_def *pid, fp32 ref, fp32 set)
{if (pid == NULL){return 0.0f;}pid->error[2] = pid->error[1];pid->error[1] = pid->error[0];pid->set = set;pid->fdb = ref;pid->error[0] = set - ref;if(fabs(pid->error[0]) <= pid->Dead_Zone)pid->error[0] = 0;if (pid->mode == PID_POSITION){pid->Pout = pid->Kp * pid->error[0];if(fabs(pid->error[0]) <= pid->Integral_Separation_Threshold)pid->Iout += pid->Ki * pid->error[0];elsepid->Iout = 0;pid->Dbuf[2] = pid->Dbuf[1];pid->Dbuf[1] = pid->Dbuf[0];pid->Dbuf[0] = (pid->error[0] - pid->error[1]);pid->Dout = pid->Kd * pid->Dbuf[0];LimitMax(pid->Iout, pid->max_iout);pid->out = pid->Pout + pid->Iout + pid->Dout;LimitMax(pid->out, pid->max_out);}else if (pid->mode == PID_DELTA){pid->Pout = pid->Kp * (pid->error[0] - pid->error[1]);if(fabs(pid->error[0]) <= pid->Integral_Separation_Threshold)pid->Iout = pid->Ki * pid->error[0];elsepid->Iout = 0;pid->Dbuf[2] = pid->Dbuf[1];pid->Dbuf[1] = pid->Dbuf[0];pid->Dbuf[0] = (pid->error[0] - 2.0f * pid->error[1] + pid->error[2]);pid->Dout = pid->Kd * pid->Dbuf[0];pid->out += pid->Pout + pid->Iout + pid->Dout;LimitMax(pid->out, pid->max_out);}return pid->out;
}/*** @brief          pid out clear* @param[out]     pid: PID struct data point* @retval         none*/
/*** @brief          pid 输出清除* @param[out]     pid: PID结构数据指针* @retval         none*/
void PID_clear(pid_type_def *pid)
{if (pid == NULL){return;}pid->error[0] = pid->error[1] = pid->error[2] = 0.0f;pid->Dbuf[0] = pid->Dbuf[1] = pid->Dbuf[2] = 0.0f;pid->out = pid->Pout = pid->Iout = pid->Dout = 0.0f;pid->fdb = pid->set = 0.0f;
}

PID.h

/******************************(C) COPYRIGHT 2016 DJI***************************** @file       pid.c/h* @brief      pid实现函数，包括初始化，PID计算函数，* @note* @history*  Version    Date            Author          Modification*  V1.0.0     Dec-26-2018     RM              1. 完成*@verbatim============================================================================================================================================================@endverbatim****************************(C) COPYRIGHT 2016 DJI*****************************/
#ifndef PID_H
#define PID_H#include <stdint.h>typedef float fp32;
typedef double fp64;enum PID_MODE
{PID_POSITION = 0,PID_DELTA
};typedef struct
{uint8_t mode;//PID 三参数fp32 Kp;fp32 Ki;fp32 Kd;fp32 max_out;  //最大输出fp32 max_iout; //最大积分输出fp32 set;fp32 fdb;fp32 out;fp32 Pout;fp32 Iout;fp32 Dout;fp32 Dbuf[3];  //微分项 0最新 1上一次 2上上次fp32 error[3]; //误差项 0最新 1上一次 2上上次fp32 Integral_Separation_Threshold;fp32 Dead_Zone;} pid_type_def;
/*** @brief          pid struct data init* @param[out]     pid: PID struct data point* @param[in]      mode: PID_POSITION: normal pid*                 PID_DELTA: delta pid* @param[in]      PID: 0: kp, 1: ki, 2:kd* @param[in]      max_out: pid max out* @param[in]      max_iout: pid max iout* @retval         none*/
/*** @brief          pid struct data init* @param[out]     pid: PID结构数据指针* @param[in]      mode: PID_POSITION:普通PID*                 PID_DELTA: 差分PID* @param[in]      PID: 0: kp, 1: ki, 2:kd* @param[in]      max_out: pid最大输出* @param[in]      max_iout: pid最大积分输出* @param[in]      Integral_Separation_Threshold :积分分离阈值* @param[in]      Dead_Zone :死区* @retval         none*/
extern void PID_init(pid_type_def *pid, uint8_t mode, const fp32 PID[3], fp32 max_out, fp32 max_iout, fp32 Integral_Separation_Threshold, fp32 Dead_Zone);/*** @brief          pid calculate* @param[out]     pid: PID struct data point* @param[in]      ref: feedback data* @param[in]      set: set point* @retval         pid out*/
/*** @brief          pid计算* @param[out]     pid: PID结构数据指针* @param[in]      ref: 反馈数据* @param[in]      set: 设定值* @retval         pid输出*/
extern fp32 PID_calc(pid_type_def *pid, fp32 ref, fp32 set);/*** @brief          pid out clear* @param[out]     pid: PID struct data point* @retval         none*/
/*** @brief          pid 输出清除* @param[out]     pid: PID结构数据指针* @retval         none*/
extern void PID_clear(pid_type_def *pid);#endif

修正

四倍频时，轮子转动一圈的脉冲数为4×减速比×编码器线数；
在这里插入图片描述

其他则，轮子转动一圈的脉冲数为2×减速比×编码器线数；
可将其设为计数上限，方便后续计算：
500线，减速比为27的电机：
在这里插入图片描述

Encoder.c

/** Encoder.c**  Created on: Jul 22, 2021*      Author: Royic*/#include "Encoder.h"
#include <math.h>int32_t Wheel_Count[4] = {0};
float Wheel_Speed[4] = {0};void Encoder_Init(TIM_HandleTypeDef *htim)
{htim->Instance->CNT=0;HAL_TIM_Encoder_Start(htim,TIM_CHANNEL_ALL);//编码器模式启动，写ALL为开始该定时器（TIMx）的通道1和通道2（编码器模式可自动计算）。使用编码器模式不用输入捕获HAL_TIM_Encoder_Start_IT(htim,TIM_CHANNEL_ALL);//开启中断__HAL_TIM_ENABLE_IT(htim,TIM_IT_UPDATE);   //使能更新中断一定要使能这一句话困扰了我两天
}float Motor_Get_Angle(uint8_t Motor_ID)
{switch(Motor_ID){case 0:return (1 - (float)Encoder_1_Htim.Instance->CNT/(ENCODER_PPR * Reduction_Ratio * ENCODER_TIMES)) * 360;case 1:return (1 - (float)Encoder_2_Htim.Instance->CNT/(ENCODER_PPR * Reduction_Ratio * ENCODER_TIMES)) * 360;case 2:return (float)Encoder_3_Htim.Instance->CNT/(ENCODER_PPR * Reduction_Ratio * ENCODER_TIMES) * 360;case 3:return (float)Encoder_4_Htim.Instance->CNT/(ENCODER_PPR * Reduction_Ratio * ENCODER_TIMES) * 360;}return 0;
}void Motor_Get_Speed()	//放到1ms的中断中
{uint8_t i;static int32_t Wheel_Count_Temp[4] = {0};static float Wheel_Angle_Temp1[4] = {0};static float Wheel_Angle_Temp2[4] = {0};for(i = 0; i < 4; ++i){Wheel_Angle_Temp2[i] = Motor_Get_Angle(i);Wheel_Speed[i] = 1000 * ((Wheel_Count[i] - Wheel_Count_Temp[i]) * 360 + Wheel_Angle_Temp2[i] - Wheel_Angle_Temp1[i]);Wheel_Count_Temp[i] = Wheel_Count[i];Wheel_Angle_Temp1[i] = Wheel_Angle_Temp2[i];}
}void Motor_Get_Turns(TIM_HandleTypeDef *htim)	//放到HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)中
{if(htim == &Encoder_1_Htim){if((htim->Instance->CR1 & 16) >> 4){++Wheel_Count[0];}else{--Wheel_Count[0];}}else if(htim == &Encoder_2_Htim){if((htim->Instance->CR1 & 16) >> 4){++Wheel_Count[1];}else{--Wheel_Count[1];}}else if(htim == &Encoder_3_Htim){if((htim->Instance->CR1 & 16) >> 4){--Wheel_Count[2];}else{++Wheel_Count[2];}}else if(htim == &Encoder_4_Htim){if((htim->Instance->CR1 & 16) >> 4){--Wheel_Count[3];}else{++Wheel_Count[3];}}
}

Encoder.h

/** Encoder.h**  Created on: Jul 22, 2021*      Author: Royic*/#ifndef ENCODER_ENCODER_H_
#define ENCODER_ENCODER_H_#include "main.h"#define ENCODER_TIMES 4
#define ENCODER_PPR 500
#define Reduction_Ratio 27#define Encoder_1_Htim		htim4
extern TIM_HandleTypeDef	Encoder_1_Htim;#define Encoder_2_Htim		htim5
extern TIM_HandleTypeDef	Encoder_2_Htim;#define Encoder_3_Htim		htim2
extern TIM_HandleTypeDef	Encoder_3_Htim;#define Encoder_4_Htim		htim3
extern TIM_HandleTypeDef	Encoder_4_Htim;extern int32_t Wheel_Count[];
extern float Wheel_Speed[4];void Encoder_Init(TIM_HandleTypeDef *htim);
void Motor_Get_Turns(TIM_HandleTypeDef *htim);//放到HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)中
float Motor_Get_Angle(uint8_t Motor_ID);
void Motor_Get_Speed();	//放到1ms的中断中#endif /* ENCODER_ENCODER_H_ */