STM32 HAL库PID控制电机

第三章 PID控制双电机

注：本文含全部PID控制代码，保证可以运行，如不能运行可以留言回复

1 基础配置

1.1 编码器电路图及配置

引脚	定时器通道
PA0	TIM2_CH1
PA1	TIM2_CH2
PB6	TIM4_CH1
PB7	TIM4_CH2

因此需要把TIM2、TIM4配置为编码器模式。在STM32CubeIDE中找到定时器2与定时器4，进行模式配置。以下以定时器2为例，定时器4只需进行相同配置即可。选择定时器为编码器模式，设置为不分频，最大计数值为65535，使能自动重装载，并选择TI1和TI2两路输入，实现四倍频效果。

配置完定时器2和定时器4后，需要再使用一个定时器，利用其产生50ms中断来读取当前的小车速度值，本次例程中采用定时器3产生中断。

周期为50ms，计算方法为：T=(arr+1)*(psc+1)/Tclk

注意： T I M 3 需要在 N V I C S e t t i n g 中开启中断 \color{red}{注意：TIM3需要在NVIC Setting中开启中断} 注意：TIM3需要在NVICSetting中开启中断

1.2 增量式PID控制

PID可以分为位置式PID与增量式PID，关于PID的具体控制原理知识不在此进行详细介绍，在这篇文章有：https://blog.csdn.net/weixin_43002939/article/details/130178782

重点介绍的为在本例程中采用的增量式PID。

增量式PID是通过改变输出量的大小来控制被控量的稳定，增量式PID与位置式PID不同，增量式返回的数值为当前时刻的控制量与上一时刻的控制量的差值，以此差值作为新的控制量进行反馈。

举个例子：设定小车的速度为0.2m/s，通过编码器进行测速得到速度反馈，与设定值产生了偏差ek，系统中保存了上一次的偏差e(k-1)还有上上次的的偏差e(k-2)，这三个值作为输入量通过增量式PID的计算公式得到Δu(k)，将上一次经过PID计算后的u(k-1)加上本次的增量Δu(k)，便得到本次控制周期的PID输出u(k)。将输出值经过二次的计算转换后，得到可以对电机转速进行控制的PWM占空比，进而对小车的运动速度进行控制。

增量式PID的公式：Kp比例系数、Ki积分系数、Kd微分系数、e(k)偏差

Δu(k)=Kp[e(k)-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)]

了解增量式PID的一些基础知识后，让我们来看看如何用代码实现算法过程吧！

2 main.c文件

/* USER CODE BEGIN Header */
/********************************************************************************* @file           : main.c* @brief          : Main program body******************************************************************************* @attention** Copyright (c) 2023 STMicroelectronics.* All rights reserved.** This software is licensed under terms that can be found in the LICENSE file* in the root directory of this software component.* If no LICENSE file comes with this software, it is provided AS-IS.********************************************************************************/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "rtc.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "../../icode/pid/pid.h"
#include "../inc/retarget.h"/* USER CODE END Includes *//* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD *//* USER CODE END PTD *//* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD *//* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM *//* USER CODE END PM *//* Private variables ---------------------------------------------------------*//* USER CODE BEGIN PV */
int  Encoder_A,Encoder_B ; //编码器的脉冲计数
float  Target_Velocity_A=30,Target_Velocity_B=30;
int Moto_A=0,Moto_B=0; //电机PWM变量 应是Motor的 向Moto致敬
float Velocity_KP_A = 1, Velocity_KI_A = 0.2, Velocity_KD_A = 0; //PID系数
float Velocity_KP_B = 1, Velocity_KI_B = 0.2, Velocity_KD_B = 0;/* USER CODE END PV *//* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP *//* USER CODE END PFP *//* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 *//* USER CODE END 0 *//*** @brief  The application entry point.* @retval int*/
int main(void)
{/* USER CODE BEGIN 1 *//* USER CODE END 1 *//* MCU Configuration--------------------------------------------------------*//* Reset of all peripherals, Initializes the Flash interface and the Systick. */HAL_Init();/* USER CODE BEGIN Init *//* USER CODE END Init *//* Configure the system clock */SystemClock_Config();/* USER CODE BEGIN SysInit *//* USER CODE END SysInit *//* Initialize all configured peripherals */MX_GPIO_Init();MX_RTC_Init();MX_TIM1_Init();MX_USART1_UART_Init();MX_TIM2_Init();MX_TIM3_Init();MX_TIM4_Init();/* USER CODE BEGIN 2 */HAL_TIM_Base_Start_IT(&htim3);                       //开启10ms定时器中断HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_4); //开启TIM1的PWMHAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);HAL_TIM_Encoder_Start(&htim2, TIM_CHANNEL_1);  //启动定时器2的编码器模式HAL_TIM_Encoder_Start(&htim2, TIM_CHANNEL_2);HAL_TIM_Encoder_Start(&htim4, TIM_CHANNEL_1);  //启动定时器4的编码器模式HAL_TIM_Encoder_Start(&htim4, TIM_CHANNEL_2);RetargetInit(&huart1);/* USER CODE END 2 *//* Infinite loop *//* USER CODE BEGIN WHILE */while (1) {/* USER CODE END WHILE *//* USER CODE BEGIN 3 */}/* USER CODE END 3 */
}/*** @brief System Clock Configuration* @retval None*/
void SystemClock_Config(void)
{RCC_OscInitTypeDef RCC_OscInitStruct = {0};RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};RCC_PeriphCLKInitTypeDef PeriphClkInit = {0};/** Initializes the RCC Oscillators according to the specified parameters* in the RCC_OscInitTypeDef structure.*/RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_LSI|RCC_OSCILLATORTYPE_HSE;RCC_OscInitStruct.HSEState = RCC_HSE_ON;RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1;RCC_OscInitStruct.HSIState = RCC_HSI_ON;RCC_OscInitStruct.LSIState = RCC_LSI_ON;RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9;if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK){Error_Handler();}/** Initializes the CPU, AHB and APB buses clocks*/RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK|RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK){Error_Handler();}PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_RTC;PeriphClkInit.RTCClockSelection = RCC_RTCCLKSOURCE_LSI;if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK){Error_Handler();}
}/* USER CODE BEGIN 4 */
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)  //定时器3中断回调函数
{if (htim == (&htim3)) {Encoder_A=Read_Velocity(2);      //===更新速度信息Encoder_B=Read_Velocity(4);Moto_A=Incremental_PI_A(CalActualSpeed(Encoder_A),Target_Velocity_A);    //===速度PID控制器Moto_B=Incremental_PI_B(CalActualSpeed(Encoder_B),Target_Velocity_B);Xianfu_Pwm();                                  //===PWM限幅Set_Pwm_A(Moto_A);Set_Pwm_B(Moto_B);
//      Set_Pwm(Target_Velocity_A);//       printf("Target_Velocity_A:%.2f \n",Target_Velocity_A);printf("%.2f,%.2f \n",CalActualSpeed(Encoder_A),CalActualSpeed(Encoder_B));//     printf("Encoder_B:%.2f \n",CalActualSpeed(Encoder_B));
//      printf("Moto_A is %d\r\n",Moto_A);//      printf("Target_Velocity_B: %.2f\r\n",Target_Velocity_B);
//      printf("Encoder_B is %d \r\n",Encoder_B);
//      printf("Moto_B is %d\r\n",Moto_B);}}
/* USER CODE END 4 *//*** @brief  This function is executed in case of error occurrence.* @retval None*/
void Error_Handler(void)
{/* USER CODE BEGIN Error_Handler_Debug *//* User can add his own implementation to report the HAL error return state */__disable_irq();while (1) {}/* USER CODE END Error_Handler_Debug */
}#ifdef  USE_FULL_ASSERT
/*** @brief  Reports the name of the source file and the source line number*         where the assert_param error has occurred.* @param  file: pointer to the source file name* @param  line: assert_param error line source number* @retval None*/
void assert_failed(uint8_t *file, uint32_t line)
{/* USER CODE BEGIN 6 *//* User can add his own implementation to report the file name and line number,ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) *//* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */

3 pid.c

/** pid.c**  Created on: 2023年4月4日*      Author: 77454*/#include "pid.h"/**************************************************************************函数功能：单位时间读取编码器计数入口参数：定时器返回  值：速度值**************************************************************************/
int Read_Velocity(uint8_t TIMX) {int Encoder_TIM;switch (TIMX) {case 2:Encoder_TIM = -(short) TIM2->CNT;TIM2->CNT = 0;break;case 3:Encoder_TIM = (short) TIM3->CNT;TIM3->CNT = 0;break;case 4:Encoder_TIM = (short) TIM4->CNT;TIM4->CNT = 0;break;default:Encoder_TIM = 0;}return Encoder_TIM;
}//计算速度  cm/s
float CalActualSpeed(int pulse) {return (float) (0.3092424 * pulse);
}/**************************************************************************函数功能：取绝对值入口参数：int返回  值：unsigned int**************************************************************************/
int myabs(int a) {int temp;if (a < 0)temp = -a;elsetemp = a;return temp;
}/**************************************************************************函数功能：赋值给PWM寄存器入口参数：PWM返回  值：无**************************************************************************/
void Set_Pwm_A(int moto_A) {if (moto_A < 0) {HAL_GPIO_WritePin(AIN1_GPIO_Port, AIN1_Pin, GPIO_PIN_SET);HAL_GPIO_WritePin(AIN2_GPIO_Port, AIN2_Pin, GPIO_PIN_RESET);} else {HAL_GPIO_WritePin(AIN1_GPIO_Port, AIN1_Pin, GPIO_PIN_RESET);HAL_GPIO_WritePin(AIN2_GPIO_Port, AIN2_Pin, GPIO_PIN_SET);}__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, myabs(moto_A));}
void Set_Pwm_B(int moto_B) {if (moto_B < 0) {HAL_GPIO_WritePin(BIN1_GPIO_Port, BIN1_Pin, GPIO_PIN_RESET);HAL_GPIO_WritePin(BIN2_GPIO_Port, BIN2_Pin, GPIO_PIN_SET);} else {HAL_GPIO_WritePin(BIN1_GPIO_Port, BIN1_Pin, GPIO_PIN_SET);HAL_GPIO_WritePin(BIN2_GPIO_Port, BIN2_Pin, GPIO_PIN_RESET);}__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, myabs(moto_B));
}
/**************************************************************************函数功能：限制PWM赋值入口参数：无返回  值：无**************************************************************************/
void Xianfu_Pwm(void) {int Amplitude = 7200;    //===PWM满幅是7200 限制在7100if (Moto_A < -Amplitude)Moto_A = -Amplitude;if (Moto_A > Amplitude)Moto_A = Amplitude;if (Moto_B < -Amplitude)Moto_B = -Amplitude;if (Moto_B > Amplitude)Moto_B = Amplitude;
}/**************************************************************************函数功能：增量PI控制器入口参数：编码器测量值，目标速度返回  值：电机PWM根据增量式离散PID公式pwm+=Kp[e（k）-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)]e(k)代表本次偏差e(k-1)代表上一次的偏差  以此类推pwm代表增量输出在我们的速度控制闭环系统里面，只使用PI控制pwm+=Kp[e（k）-e(k-1)]+Ki*e(k)**************************************************************************/
int Incremental_PI_A(float Encoder, float Target) {static float Bias_A, Pwm_A, Last_bias_A;Bias_A = Target - Encoder;                                  //计算偏差Pwm_A += Velocity_KP_A * (Bias_A - Last_bias_A) + Velocity_KI_A * Bias_A; //增量式PI控制器Last_bias_A = Bias_A;                                    //保存上一次偏差return Pwm_A;                                           //增量输出
}int Incremental_PI_B(float Encoder, float Target) {static float Bias, Pwm, Last_bias;Bias = Target - Encoder;                                  //计算偏差Pwm += Velocity_KP_B * (Bias - Last_bias) + Velocity_KI_B * Bias; //增量式PI控制器Last_bias = Bias;                                       //保存上一次偏差return Pwm;                                           //增量输出
}

4 pid.h

/** pid.h**  Created on: 2023年4月4日*      Author: 77454*/#ifndef PID_PID_H_
#define PID_PID_H_#include "main.h"
#include "tim.h"extern int Encoder_A;
extern float Target_Velocity_A;
extern int Moto_A;
extern float Velocity_KP_A;
extern float Velocity_KI_A;
extern float Velocity_KD_A;extern int Encoder_B;
extern float Target_Velocity_B;
extern int Moto_B;
extern float Velocity_KP_B;
extern float Velocity_KI_B;
extern float Velocity_KD_B;int Read_Velocity(uint8_t TIMX);
int myabs(int a);
void Set_Pwm_A(int moto_A);
void Set_Pwm_B(int moto_B);
int Incremental_PI_A(float Encoder,float Target);
int Incremental_PI_B(float Encoder, float Target);void Xianfu_Pwm(void);
float CalActualSpeed(int pulse);#endif /* PID_PID_H_ */