STM32 HAL库PID控制电机 第三章 PID控制双电机_双电机pid控制

STM32 HAL库PID控制电机

第三章 PID控制双电机

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

1 基础配置

1.1 编码器电路图及配置

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


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

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

$注意：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的PWM
	HAL_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 */

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
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241

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;
	else
		temp = 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 限制在7100
	if (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;                                           //增量输出
}


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
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127

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_ */

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
43