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

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

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

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;                                           //增量输出
}

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