STM32控制编码器电机实现【速度闭环控制】与【位置闭环控制】
一、硬件及接线说明
本实验所基于的硬件分别为：
- STM32F103C8T6 主控板
- TB6612FNG 直流电机驱动模块
- 6线正交编码器电机（带AB相）
其中硬件接线为：
- PWMA —— PA8
- AIN1 —— PB14
- AIN2 —— PB15
- STBY —— 5V
- 编码器A相 —— PA1
- 编码器B相 —— PA0
STM32定时器资源分配：
- 定时器1（TIM1）：产生PWM波，作为TB6612的输入，控制电机进行调速；
- 定时器2（TIM2）：读取编码器的波形；
- 定时器3（TIM3）：产生周期为10ms的定时器中断，为控制系统提供稳定的时间基准。
【说明】上述硬件平台和接线仅给读者提供参考，更换主控或接线方式，请自行对示例程序进行微调。本文对于编码器的工作原理不加赘述，对于其原理请读者自行查阅相关资料。
二、速度闭环控制程序逻辑
【说明】下述程序中 control.c 最为重要，包含了速度闭环控制器的详细代码。其他程序模块供读者初始化参考。
main.c （主函数）
u8 flag_Stop=1; //停止标志位
int Encoder; //编码器的脉冲计数
int moto; //电机PWM变量
int main(void)
{
Stm32_Clock_Init(9); //系统时钟设置
delay_init(); //=====延时初始化
LED_Init(); //=====初始化与 LED 连接的硬件接口
uart_init(115200); //=====初始化串口1
MOTO_Init(); //初始化控制电机所需的IO
pwm_Init(7199,0); //初始化pwm输出
Encoder_Init_TIM2(); //初始化计数器（定时器）
TIM3_Int_Init(99,7199); //10ms一次中断
while(1)
{
printf("Encoder:%d \r\n",Encoder);
}
}
moto.c （电机初始化相关函数）
void MOTO_Init(void)//初始化控制电机所需的IO
{
GPIO_InitTypeDef GPIO_InitStruct;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);
//PORTB12 13 14 15推挽输出
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStruct.GPIO_Pin = GPIO_Pin_12|GPIO_Pin_13|GPIO_Pin_14|GPIO_Pin_15;
GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOB,&GPIO_InitStruct);
}
void pwm_Init(u16 arr,u16 psc) //初始化pwm输出引脚
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
TIM_OCInitTypeDef TIM_OCInitStruct;
GPIO_InitTypeDef GPIO_InitStruct;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1,ENABLE); //使能定时器1时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE); //使能GPIOA的时钟
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF_PP; //复用输出
GPIO_InitStruct.GPIO_Pin = GPIO_Pin_8|GPIO_Pin_11; //PA8 PA11
GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA,&GPIO_InitStruct);
TIM_TimeBaseInitStruct.TIM_Period = arr; //设定计数器自动重装值
TIM_TimeBaseInitStruct.TIM_Prescaler = psc; //设定预分频器
TIM_TimeBaseInitStruct.TIM_CounterMode = TIM_CounterMode_Up; //TIM向上计数模式
TIM_TimeBaseInitStruct.TIM_ClockDivision = 0; //设置时钟分割
TIM_TimeBaseInit(TIM1,&TIM_TimeBaseInitStruct); //初始化定时器
TIM_OCInitStruct.TIM_OCMode = TIM_OCMode_PWM2; //选择PWM2模式
TIM_OCInitStruct.TIM_OutputState = TIM_OutputState_Enable; //比较输出使能
TIM_OCInitStruct.TIM_Pulse = 0; //设置待装入捕获比较寄存器的脉冲值
TIM_OCInitStruct.TIM_OCPolarity = TIM_OCPolarity_High; //设置输出极性
TIM_OC1Init(TIM1,&TIM_OCInitStruct); //初始化输出比较参数
TIM_OC1PreloadConfig(TIM1,TIM_OCPreload_Enable); //CH1使能预装载寄存器
TIM_CtrlPWMOutputs(TIM1,ENABLE); //高级定时器输出必须设置这句
TIM_ARRPreloadConfig(TIM1, ENABLE); //使能TIM1在ARR上的预装载寄存器
TIM_Cmd(TIM1,ENABLE); //使能定时器1
}
encoder.c （编码器初始化函数）
#include "encoder.h"
/**************************************************************************
函数功能：把TIM2初始化为编码器接口模式
入口参数：无
返回 值：无
**************************************************************************/
void Encoder_Init_TIM2(void)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_ICInitTypeDef TIM_ICInitStructure;
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);//使能定时器2的时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);//使能PA端口时钟
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0|GPIO_Pin_1; //端口配置
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; //浮空输入
GPIO_Init(GPIOA, &GPIO_InitStructure); //根据设定参数初始化GPIOB
TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
TIM_TimeBaseStructure.TIM_Prescaler = 0x0; // 预分频器
TIM_TimeBaseStructure.TIM_Period = ENCODER_TIM_PERIOD-1; //设定计数器自动重装值
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;//选择时钟分频：不分频
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_CenterAligned1;TIM向上计数
TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);
TIM_EncoderInterfaceConfig(TIM2, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);//使用编码器模式3
TIM_ICStructInit(&TIM_ICInitStructure); //把TIM_ICInitStruct 中的每一个参数按缺省值填入
TIM_ICInitStructure.TIM_ICFilter = 10; //设置滤波器长度
TIM_ICInit(TIM2, &TIM_ICInitStructure); //根据 TIM_ICInitStruct 的参数初始化外设 TIMx
TIM_ClearFlag(TIM2, TIM_FLAG_Update);//清除TIM的更新标志位
TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE);//使能定时器中断
TIM_SetCounter(TIM2,0);//设置TIMx 计数器寄存器值
TIM_Cmd(TIM2, ENABLE); //使能定时器2
}
//中断处理函数为空，清除中断标志位后结束中断
void TIM2_IRQHandler(void)
{
if(TIM_GetFlagStatus(TIM2,TIM_FLAG_Update)==SET)//溢出中断
{
TIM_ClearITPendingBit(TIM2,TIM_IT_Update); //清除中断标志位
}
}
/**************************************************************************
函数功能：单位时间读取编码器计数
入口参数：定时器
返回 值：速度值
**************************************************************************/
int Read_Encoder(u8 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;
}
timer.c （定时器中断初始化函数）
/**************************************************************************
函数功能：定时中断初始化
入口参数：arr：自动重装值 psc：时钟预分频数
返回 值：无
**************************************************************************/
void TIM3_Int_Init(u16 arr,u16 psc)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
NVIC_InitTypeDef NVIC_InitStruct;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3,ENABLE);
TIM_TimeBaseInitStruct.TIM_Period = arr; //重装载值
TIM_TimeBaseInitStruct.TIM_Prescaler = psc; //预分频系数
TIM_TimeBaseInitStruct.TIM_ClockDivision =0; //时钟分割
TIM_TimeBaseInitStruct.TIM_CounterMode = TIM_CounterMode_Up; //TIM向上计数模式
TIM_TimeBaseInit(TIM3,&TIM_TimeBaseInitStruct);
TIM_ITConfig(TIM3,TIM_IT_Update,ENABLE); //使能定时器中断
NVIC_InitStruct.NVIC_IRQChannel = TIM3_IRQn; //使能外部中断通道
NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE; //使能外部中断通道
NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = 1; //抢占优先级1
NVIC_InitStruct.NVIC_IRQChannelSubPriority = 3; //响应优先级3
NVIC_Init(&NVIC_InitStruct);
TIM_Cmd(TIM3,ENABLE); //使能定时器3
}
control.c (核心控制程序，此模块程序可供读者详细阅读)
#include "control.h"
int Target_velocity=50; //设定速度控制的目标速度为50个脉冲每10ms
int TIM3_IRQHandler(void)
{
if(TIM_GetFlagStatus(TIM3,TIM_FLAG_Update)==SET)
{
TIM_ClearITPendingBit(TIM3,TIM_IT_Update); //===清除定时器3中断标志位
Encoder=Read_Encoder(2); //取定时器2计数器的值
Led_Flash(100); //LED闪烁
moto=Incremental_PI(Encoder,Target_velocity); //===速度PID控制器
Xianfu_Pwm();
Set_Pwm(moto);
}
return 0;
}
/**************************************************************************
函数功能：赋值给PWM寄存器
入口参数：PWM
返回 值：无
**************************************************************************/
void Set_Pwm(int moto)//赋值给PWM寄存器
{
if(moto>0) AIN1=0, AIN2=1;
else AIN1=1, AIN2=0;
PWMA=myabs(moto);
}
/**************************************************************************
函数功能：限制PWM赋值
入口参数：无
返回 值：无
**************************************************************************/
void Xianfu_Pwm(void) //限制幅度的函数
{
int Amplitude=7100; //===PWM满幅是7200 限制在7100
if(moto<-Amplitude) moto = -Amplitude;
if(moto>Amplitude) moto = Amplitude;
}
/**************************************************************************
函数功能：绝对值函数
入口参数：int
返回 值：unsigned int
**************************************************************************/
int myabs(int a) //取绝对值
{
int temp;
if(a<0) temp=-a;
else temp=a;
return temp;
}
/**************************************************************************
函数功能：增量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 (int Encoder,int Target)
{
float Kp=20,Ki=30;
static int Bias,Pwm,Last_bias;
Bias=Encoder-Target; //计算偏差
Pwm+=Kp*(Bias-Last_bias)+Ki*Bias; //增量式PI控制器
Last_bias=Bias; //保存上一次偏差
return Pwm; //增量输出
}
三、位置闭环控制程序逻辑
【说明】下述程序中 control.c 最为重要，包含了速度闭环控制器的详细代码。其他程序模块供读者初始化参考。
main.c
u8 flag_Stop=1; //停止标志位
int Encoder,Position=10000; //编码器的脉冲计数
int moto; //电机PWM变量
int main(void)
{
Stm32_Clock_Init(9); //系统时钟设置
delay_init(); //=====延时初始化
LED_Init(); //=====初始化与 LED 连接的硬件接口
uart_init(115200); //=====初始化串口1
MOTO_Init(); //初始化控制电机所需的IO
pwm_Init(7199,0); //初始化pwm输出
Encoder_Init_TIM2(); //初始化计数器（定时器）
TIM3_Int_Init(99,7199); //10ms一次中断
while(1)
{
printf("Encoder:%d Position:%d \r\n",Encoder,Position);
}
}
moto.c
void MOTO_Init(void)//初始化控制电机所需的IO
{
GPIO_InitTypeDef GPIO_InitStruct;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);//PORTB12 13 14 15推挽输出
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStruct.GPIO_Pin = GPIO_Pin_12|GPIO_Pin_13|GPIO_Pin_14|GPIO_Pin_15;
GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOB,&GPIO_InitStruct);
}
void pwm_Init(u16 arr,u16 psc) //初始化pwm输出引脚
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
TIM_OCInitTypeDef TIM_OCInitStruct;
GPIO_InitTypeDef GPIO_InitStruct;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1,ENABLE); //使能定时器1时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE); //使能GPIOA的时钟
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF_PP; //复用输出
GPIO_InitStruct.GPIO_Pin = GPIO_Pin_8|GPIO_Pin_11; //PA8 PA11
GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA,&GPIO_InitStruct);
TIM_TimeBaseInitStruct.TIM_Period = arr; //设定计数器自动重装值
TIM_TimeBaseInitStruct.TIM_Prescaler = psc; //设定预分频器
TIM_TimeBaseInitStruct.TIM_CounterMode = TIM_CounterMode_Up; //TIM向上计数模式
TIM_TimeBaseInitStruct.TIM_ClockDivision = 0; //设置时钟分割
TIM_TimeBaseInit(TIM1,&TIM_TimeBaseInitStruct); //初始化定时器
TIM_OCInitStruct.TIM_OCMode = TIM_OCMode_PWM2; //选择PWM2模式
TIM_OCInitStruct.TIM_OutputState = TIM_OutputState_Enable; //比较输出使能
TIM_OCInitStruct.TIM_Pulse = 0; //设置待装入捕获比较寄存器的脉冲值
TIM_OCInitStruct.TIM_OCPolarity = TIM_OCPolarity_High; //设置输出极性
TIM_OC1Init(TIM1,&TIM_OCInitStruct); //初始化输出比较参数
TIM_OC1PreloadConfig(TIM1,TIM_OCPreload_Enable); //CH1使能预装载寄存器
TIM_CtrlPWMOutputs(TIM1,ENABLE); //高级定时器输出必须设置这句
TIM_ARRPreloadConfig(TIM1, ENABLE); //使能TIM1在ARR上的预装载寄存器
TIM_Cmd(TIM1,ENABLE); //使能定时器1
}
encoder.c
void Encoder_Init_TIM2(void)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_ICInitTypeDef TIM_ICInitStructure;
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);//使能定时器2的时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);//使能PA端口时钟
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0|GPIO_Pin_1; //端口配置
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; //浮空输入
GPIO_Init(GPIOA, &GPIO_InitStructure); //根据设定参数初始化GPIOB
TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
TIM_TimeBaseStructure.TIM_Prescaler = 0x0; // 预分频器
TIM_TimeBaseStructure.TIM_Period = ENCODER_TIM_PERIOD-1; //设定计数器自动重装值
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;//选择时钟分频：不分频
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_CenterAligned1;TIM向上计数
TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);
TIM_EncoderInterfaceConfig(TIM2, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);//使用编码器模式3
TIM_ICStructInit(&TIM_ICInitStructure); //把TIM_ICInitStruct 中的每一个参数按缺省值填入
TIM_ICInitStructure.TIM_ICFilter = 10; //设置滤波器长度
TIM_ICInit(TIM2, &TIM_ICInitStructure);//根据 TIM_ICInitStruct 的参数初始化外设 TIMx
TIM_ClearFlag(TIM2, TIM_FLAG_Update);//清除TIM的更新标志位
TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE);//使能定时器中断
TIM_SetCounter(TIM2,10000);//设置TIMx 计数器寄存器值
TIM_Cmd(TIM2, ENABLE); //使能定时器2
}
//中断处理函数为空，清除中断标志位后结束中断
void TIM2_IRQHandler(void)
{
if(TIM_GetFlagStatus(TIM2,TIM_FLAG_Update)==SET)//溢出中断
{
TIM_ClearITPendingBit(TIM2,TIM_IT_Update);//清除中断标志位
}
}
/**************************************************************************
函数功能：单位时间读取编码器计数
入口参数：定时器
返回 值：速度值
**************************************************************************/
int Read_Encoder(u8 TIMX)//读取计数器的值
{
int Encoder_TIM;
switch(TIMX)
{
case 2:Encoder_TIM=(short)TIM2->CNT; break;
case 3:Encoder_TIM=(short)TIM3->CNT; break;
case 4:Encoder_TIM=(short)TIM4->CNT; break;
default: Encoder_TIM=0;
}
return Encoder_TIM;
}
timer.c
#include "timer.h"
/**************************************************************************
函数功能：定时中断初始化
入口参数：arr：自动重装值 psc：时钟预分频数
返回 值：无
**************************************************************************/
void TIM3_Int_Init(u16 arr,u16 psc)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
NVIC_InitTypeDef NVIC_InitStruct;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3,ENABLE);
TIM_TimeBaseInitStruct.TIM_Period = arr; //重装载值
TIM_TimeBaseInitStruct.TIM_Prescaler = psc; //预分频系数
TIM_TimeBaseInitStruct.TIM_ClockDivision =0; //时钟分割
TIM_TimeBaseInitStruct.TIM_CounterMode = TIM_CounterMode_Up; //TIM向上计数模式
TIM_TimeBaseInit(TIM3,&TIM_TimeBaseInitStruct);
TIM_ITConfig(TIM3,TIM_IT_Update,ENABLE); //使能定时器中断
NVIC_InitStruct.NVIC_IRQChannel = TIM3_IRQn; //使能外部中断通道
NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE; //使能外部中断通道
NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = 1; //抢占优先级1
NVIC_InitStruct.NVIC_IRQChannelSubPriority = 3; //响应优先级3
NVIC_Init(&NVIC_InitStruct);
TIM_Cmd(TIM3,ENABLE); //使能定时器3
}
control.c (核心控制程序，此模块程序可供读者详细阅读)
#include "control.h"
int Target_position=11000; //初始值是10000，目标值是11000
int TIM3_IRQHandler(void)
{
if(TIM_GetFlagStatus(TIM3,TIM_FLAG_Update)==SET)
{
TIM_ClearITPendingBit(TIM3,TIM_IT_Update); //===清除定时器1中断标志位
Encoder=Read_Encoder(2); //取定时器2计数器的值
Led_Flash(100); //LED闪烁
moto=Position_PID(Encoder,Target_position); //===位置PID控制器
Xianfu_Pwm();
Set_Pwm(moto);
}
return 0;
}
/**************************************************************************
函数功能：赋值给PWM寄存器
入口参数：PWM
返回 值：无
**************************************************************************/
void Set_Pwm(int moto)//赋值给PWM寄存器
{
if(moto>0) AIN1=0, AIN2=1;
else AIN1=1, AIN2=0;
PWMA=myabs(moto);
}
/**************************************************************************
函数功能：限制PWM赋值
入口参数：无
返回 值：无
**************************************************************************/
void Xianfu_Pwm(void) //限制幅度的函数
{
int Amplitude=7100; //===PWM满幅是7200 限制在7100
if(moto<-Amplitude) moto = -Amplitude;
if(moto>Amplitude) moto = Amplitude;
}
/**************************************************************************
函数功能：绝对值函数
入口参数：int
返回 值：unsigned int
**************************************************************************/
int myabs(int a) //取绝对值
{
int temp;
if(a<0) temp=-a;
else temp=a;
return temp;
}
/**************************************************************************
函数功能：位置式PID控制器
入口参数：编码器测量位置信息，目标位置
返回 值：电机PWM
根据位置式离散PID公式
pwm=Kp*e(k)+Ki*∑e(k)+Kd[e（k）-e(k-1)]
e(k)代表本次偏差
e(k-1)代表上一次的偏差
∑e(k)代表e(k)以及之前的偏差的累积和;其中k为1,2,,k;
pwm代表输出
**************************************************************************/
int Position_PID (int Encoder,int Target)
{
float Position_KP=80,Position_KI=0.1,Position_KD=500;
static float Bias,Pwm,Integral_bias,Last_Bias;
Bias=Encoder-Target; //计算偏差
Integral_bias+=Bias; //求出偏差的积分
Pwm=Position_KP*Bias+Position_KI*Integral_bias+Position_KD*(Bias-Last_Bias); //位置式PID控制器
Last_Bias=Bias; //保存上一次偏差
return Pwm; //增量输出
}
四、有关速度闭环控制与位置闭环控制的区别
- 通常，我们一般在移动机器人的车轮控制上会用到速度闭环控制，在倒立摆上会用到位置闭环控制。细心的读者对比上述程序可以发现：速度闭环控制使用了PI控制器，位置闭环控制使用了PID控制器，这也是两者最大区别之一。
- 完整的示例程序还在整理当中，整理完毕后会发布在GitHub上。