HAL库增量式PID算法闭环控制电机转速

只看该作者 · 2021-6-8 11:14

1、硬件准备

（1）所需硬件

芯片：STM32F103RCT6
驱动：电机驱动板或TB6612（电机驱动芯片）
电池：12V Libo电池
电机：带编码器电机
轮子：我用的是麦克纳姆轮
（2）硬件连接：

PA8 —— 电机驱动板PWM1
GND —— 电机驱动板PWM2
（这里若要反转则PWM1接地，PWM2接PA8；如果在小车中两者都需接PWM引脚，正转：PWM1给一定占空比，PWM2占空比为0）
PA9(USART1_RX) —— 接串口TX
PA10(USART1_TX) —— 接串口RX
PB6 —— 编码器A
PB7 —— 编码器B

只看该作者 · 2021-6-8 11:15

2、STM32CubeMX配置：
1.1 所用工具：

芯片：STM32F103RCT6
IDE：MDK-Keil软件
STM32F1xxHAL库
1.2 知识概括：

STM32CubeMX创建TIMx、USART例程
Keil软件程序编写
1.3 工程创建
1、芯片选择
芯片：STM32F103RCT6（根据自己的板子来进行选择）

只看该作者 · 2021-6-8 11:18

2、设置RCC
设置高速外部时钟HSE 选择外部时钟源

只看该作者 · 2021-6-8 11:20

3、LED1配置
我使用的板子LED1引脚为PD2，初始电平为高电平，目的是通过观察小灯的亮灭判断是否进入定时器中断

只看该作者 · 2021-6-8 11:21

4、USART1配置
异步收发，波特率默认：115200 Bit/s

只看该作者 · 2021-6-8 11:22

5、PWM配置
使用定时器1通道1和通道4（TIM1_CH1和TIM1_CH4），频率10KHz

只看该作者 · 2021-6-8 11:23

6、定时器中断配置
使用定时器2（TIM2），周期设为10ms，即10ms进一次定时器中断

打开更新定时器中断

只看该作者 · 2021-6-8 11:24

7、编码器配置
STM32自带编码器配置，使用定时器4（TIM4_CH1和TIM4_CH2），打开更新定时器中断

只看该作者 · 2021-6-8 11:25

8、中断优先级配置
因为编码器中断要发生在定时10ms中断内，故编码器中断的抢占优先级要大于定时10ms

只看该作者 · 2021-6-8 11:26

9、配置时钟
F1系列芯片系统时钟为72MHzs

只看该作者 · 2021-6-8 11:29

10、项目创建最后步骤

设置项目名称
选择所用IDE

只看该作者 · 2021-6-8 11:30

11、输出文件

②处：复制所用文件的.c和.h
③处：每个功能生产独立的.c和.h文件

只看该作者 · 2021-6-8 11:37

12、创建工程文件
点击GENERATE CODE 创建工程

13、配置下载工具
这里我们需要勾选上下载后直接运行，然后进行一次编译

只看该作者 · 2021-6-8 11:38

3、STM32源代码：
（1）该程序中需要加入增量式PID算法，所以我新建了一个函数control.c，专门用于写PID算法、编码器读取和电机速度控制。该函数中每段代码我有加有注释，方便大家理解。

control.c

#include "control.h"
#include "tim.h"
#include "main.h"
#include "math.h"

unsigned int MotorSpeed;  // 全局变量，电机当前速度数值，从编码器中获取
int SpeedTarget = 5000;    // 全局变量，速度目标值
int MotorOutput;    // 全局变量，电机输出

// 1.通过TIM4读取电机脉冲并计算速度
void GetMotorPulse(void)
{
  // TIM4计数器获得电机脉冲，该电机在10ms采样的脉冲/18则为实际转速的rpm
  MotorSpeed = (short)(__HAL_TIM_GET_COUNTER(&htim4)/18*7200/100);
  __HAL_TIM_SET_COUNTER(&htim4,0);  // 计数器清零
}

// 2.增量式PID控制器
int Error_Last,Error_Prev;       // 上一次偏差值，上上次误差
int Pwm_add,Pwm;                   // PWM增量，PWM输出占空比
float Kp = 2, Ki = 3, Kd = 2;    // PID系数，这里只用到PI控制器

int SpeedInnerControl(int Speed,int Target) // 速度内环控制
{
int Error = Speed - Target;    // 偏差 = 目标速度 - 实际速度

Pwm_add = Kp * (Error - Error_Last) +    // 比例
   Ki * Error +            // 积分
   Kd * (Error - 2.0f * Error_Last + Error_Prev)    // 微分
            +1;  // 加一的目的是如果输出信号为0时，系统将进入失控状态

Pwm += Pwm_add;                // 原始量+增量 = 输出量

Error_Prev = Error_Last;          // 保存上上次误差
Error_Last = Error;                // 保存上次偏差

if(Pwm > 7100) Pwm = 7100;       // 限幅
if(Pwm <-7100) Pwm = -7100;

return Pwm;                         // 返回输出值
}

//3.电机电压和方向控制函数
void SetMotorVoltageAndDirection(int Pwm)
{
  __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, Pwm);
}

只看该作者 · 2021-6-8 11:39

control.h

#ifndef __CONTROL_H
#define __CONTROL_H

//全局变量
extern unsigned int MotorSpeed; // 电机速度
extern int SpeedTarget;       // 目标速度
extern int MotorOutput;       // 电机输出

//函数声明
void GetMotorPulse(void);                   // 通过TIM4读取电机脉冲并计算速度
int SpeedInnerControl(int Speed,int Target);  // 增量式PID控制器
void SetMotorVoltageAndDirection(int Pwm); // 电机电压和方向控制函数

#endif

只看该作者 · 2021-6-8 11:41

（2）在main.c加上：

/* USER CODE BEGIN Includes */
#include "stdio.h"
#include "control.h"
/* USER CODE END Includes */

/* USER CODE BEGIN 0 */
int fputc(int ch, FILE *p)
{
while(!(USART1->SR & (1<<7)));
USART1->DR = ch;

return ch;
}
/* USER CODE END 0 */

  /* USER CODE BEGIN 2 */
  HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);       // TIM1_CH1(pwm)
  HAL_TIM_Encoder_Start(&htim4, TIM_CHANNEL_1); // 开启编码器A
  HAL_TIM_Encoder_Start(&htim4, TIM_CHANNEL_2); // 开启编码器B
  HAL_TIM_Base_Start_IT(&htim2);             // 使能定时器2中断
  /* USER CODE END 2 */

/* USER CODE BEGIN 4 */
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
static unsigned char i = 0;
if (htim == (&htim2))
{
      // 1.获取电机速度
      GetMotorPulse();

      // 2.PID控制器，取回占空比
      MotorOutput = SpeedInnerControl(MotorSpeed,SpeedTarget);

      // 3.将占空比导入至电机控制函数
      SetMotorVoltageAndDirection(MotorOutput);
      i++;
      if(i>100)
      {
      HAL_GPIO_TogglePin(GPIOD, GPIO_PIN_2);
      // 打印定时器4的计数值，short（-32768——32767）
      printf("Encoder = %d moto = %d \r\n",MotorSpeed,MotorOutput);
      i=0;
      }
}
}
/* USER CODE END 4 */

只看该作者 · 2021-6-8 11:42

[size=3font]（2）烧入STM32中，串口展示：

只看该作者 · 2021-6-8 11:44

[size=3font]（3）实物展示：

HAL库 增量式PID算法闭环控制电机转速

HAL库增量式PID算法闭环控制电机转速