简介

位置环 PID 控制的原理非常简单，我们用编码器计数总值代表电机的位置，然后把 PID控制流程中的控制对象换成电机位置即可。

步骤

• ①设定一个目标位置值
• ②读取当前电机位置值
• ③对所读到的位置数据进行pid计算
• ④将计算结果设置为电机速度目标值
• ⑤对电机当前速度数据进行pid算法计算
• ⑥根据计算结果配置PWM进而控制电机

第⑧的速度闭环控制为增量式 PID 控制，增量相当于加速度，由加速度改变速度，体现在速度控制值自增部分：

ControlVelocity+=Velcity_Kp_(Bias-Last_bias)+Velcity_Ki_Bias;

而这个实验的位置闭环控制，则相当于输出直接为目标速度，由速度改变位置，不需要增量控制。

第⑧节速度闭环控制的当前速度值直接使用编码器读数 Encoder 即可。当前位置值获取对 Encoder 进行累加，相当于对速度进行积分得

到位移。该操作将在 TIM2 定时中断服务函数中进行。

位置闭环部分代码

Bias=TargetPosition-CurrentPosition; //求位置偏差
Integral_Bias+=Bias;
ControlVelocity=Position_Kp*Bias + Position_Ki*Integral_Bias +Position_Kd*(Bias-Last_bias);

• 其中 Bias 是位置偏差，Integral_Bias 是偏差积分。下面介绍一下 Kp、Ki、Kd 部分的作用。
• Position_Kp_Bias：偏差越大，速度(ControlVelocity)越大，使电机尽快到达目标位置。_
• _Position_Ki_Integral_Bias：偏差积分，用于减小稳态误差。例如当有外力阻挡电机，使电机无法达到目标位置时，偏差不断积分，加大转矩，克服外力。同时也有解决电机控制 PWM 死区的作用。直流电机在 PWM 很小时，会直接停止不再驱动，特别是在电源电压较小时，这一死区现象会更加明显。
• Position_Kd*(Bias-Last_bias)：(Bias-Last_bias)相当于速度(偏差变小的速度，相当于靠近目标位置的速度)，只是极性相反，就可以用于限制速度，提高控制稳定性。

编码器

编码器代码说明

相关变量定义

extern int Encoder, CurrentPosition; //当前速度、当前位置
extern int TargetVelocity, TargetCircle, CurrentPosition, Encoder,PWM; //=目标速度、目标圈数、编码器读数、PWM 控制变量
extern float Velcity_Kp, Velcity_Ki, Velcity_Kd; //相关速度 PID 参数
extern float Position_Kp, Position_Ki, Position_Kd; //相关位置 PID 参数
extern int MortorRun; //允许电机控制标志位

TIM2 定时中断服务函数

速度积分获取当前位置，根据电机使能标志位决定进行位置闭环控制或者电机停止。

void TIM2_IRQHandler()
{
    if(TIM_GetITStatus(TIM2, TIM_IT_Update)==1) //当发生中断时状态寄存器(TIMx_SR)的 bit0 会被硬件置 1
    {
        Encoder=Read_Encoder(); //读取当前编码器读数，即速度
        CurrentPosition+=Encoder; //编码器读数(速度)积分得到位置
        if(MortorRun) //如果按键按下，运行电机控制程序
        {
            PWM=Position_FeedbackControl(TargetCircle, CurrentPosition);
            //位置闭环控制
            SetPWM(PWM); //设置 PWM
        }
        else 
            PWM=0,SetPWM(PWM); //如果按键再次按下，电机停止
        TIM_ClearITPendingBit(TIM2, TIM_IT_Update); //状态寄存器(TIMx_SR)的bit0 置 0
    }
}

位置闭环控制函数

使用电机的电机误差系数 1.04

int Position_FeedbackControl(float Circle, int CurrentPosition)
{
    float TargetPosition,Bias, ControlVelocity; //定义相关变量
    static float Last_bias, Integral_Bias; //静态变量，函数调用结束后其值依然存在
    TargetPosition=Circle*1040; //目标位置=目标圈数*1040
    //10ms 读取一次编码器(即 100HZ)，电机减速比 为 20，霍尔编码器精度 13，AB 双相组合得到 4 倍频，
    //则转 1 圈编码器读数为 20*13*4=1040，电机转速=Encoder*100/1040r/s 使用定时器 2
    //1.04 是误差系数，电机本身存在误差，可根据实 际情况调整该系数以提高控制精度
    Bias=TargetPosition-CurrentPosition; //求位置偏差
    Integral_Bias+=Bias;
    if(Integral_Bias> 970) Integral_Bias= 970; //积分限幅 防止到达目标位置 后过冲
    if(Integral_Bias<-970) Integral_Bias=-970; //积分限幅 防止到达目标位置后过冲
    ControlVelocity=Position_Kp*Bias+Position_Ki*Integral_Bias+Position_Kd*(Bias-Last_bias); //增量式 PI 控制器
    //Position_Kp*Bias 偏差越大速度越大
    //Position_Ki*Integral_Bias 减小稳态误差
    //Position_Kd*(Bias-Last_bias) 限制速度
    Last_bias=Bias;
    return ControlVelocity; //返回速度控制值
}

位置环PID调参

位置环调参和速度环有很大区别，按经验来说，一般用不到I和D，我们只要调整P就好。

从0开始逐步增大P，直到电机在前往目标位置的过程中是满速，而到达目标位置后不会超调、震荡就行，位置环调好后曲线。

实验操作

• 编码器初始化函数使能 GPIO、TIM 时钟→初始化 GPIO 引脚→初始化定时器→初始化编码器→清除相关标志位、数值→使能相关功能、定时器。
• 读取编码器计数值函数读取编码器计数并判断方向正反，读取完后将计数值清零。
• 定时读取编码器数值初始化函数使能TIM2时钟→初始化定时器→使能中断→初始化中断分组→使能定时器。
• TIM2 中断服务函数调用读取编码器计数值函数并进行位置闭环控制。
• 电机将进行位置闭环控制：旋转 10 圈。同时 OLED 显示屏会显示目标位置值(Target_P)、当前位置值(Current_P)目标速度值(Target_V)、读取速度值(Current_V)和当前 PWM 值。

Code

encoder.c

#include "motorencoder.h" 
extern int   Encoder, CurrentPosition; //当前速度、当前位置
extern int   TargetVelocity, TargetCircle, CurrentPosition, Encoder,PWM; //目标速度、目标圈数、编码器读数、PWM控制变量
extern float Velcity_Kp,  Velcity_Ki,  Velcity_Kd; //相关速度PID参数
extern float Position_Kp, Position_Ki, Position_Kd; //相关位置PID参数
extern int   MortorRun;  //允许电机控制标志位

/**************************************************************************
函数功能：编码器初始化函数
入口参数：无
返回  值：无
**************************************************************************/
void MotorEncoder_Init(void)
{
    GPIO_InitTypeDef GPIO_InitStructure; //定义一个引脚初始化的结构体  
    TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;//定义一个定时器初始化的结构体
    TIM_ICInitTypeDef TIM_ICInitStructure; //定义一个定时器编码器模式初始化的结构体

    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); //使能TIM4时钟
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); //使能CPIOB时钟

    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7;    //TIM4_CH1、TIM4_CH2
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; //浮空输入
    GPIO_Init(GPIOB, &GPIO_InitStructure);    //根据GPIO_InitStructure的参数初始化GPIO

    TIM_TimeBaseStructure.TIM_Period = 0xffff; //设定计数器自动重装值
    TIM_TimeBaseStructure.TIM_Prescaler = 0; // 预分频器 
    TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; //选择时钟分频：不分频
    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; //TIM向上计数模式
    TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure); //根据TIM_TimeBaseInitStruct的参数初始化定时器TIM4

    TIM_EncoderInterfaceConfig(TIM4, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising); //使用编码器模式3：CH1、CH2同时计数，为四分频
    TIM_ICStructInit(&TIM_ICInitStructure); //把TIM_ICInitStruct 中的每一个参数按缺省值填入
    TIM_ICInitStructure.TIM_ICFilter = 10;  //设置滤波器长度
    TIM_ICInit(TIM4, &TIM_ICInitStructure); //根TIM_ICInitStructure参数初始化定时器TIM4编码器模式

    TIM_ClearFlag(TIM4, TIM_FLAG_Update);//清除TIM的更新标志位
    TIM_ITConfig(TIM4, TIM_IT_Update, ENABLE); //更新中断使能
    TIM_SetCounter(TIM4,0); //初始化清空编码器数值

    TIM_Cmd(TIM4, ENABLE); //使能定时器4
}

/**************************************************************************
函数功能：读取TIM4编码器数值
入口参数：无
返回  值：无
**************************************************************************/
int Read_Encoder(void)
{
    int Encoder_TIM;
    Encoder_TIM=TIM4->CNT; //读取计数
    if(Encoder_TIM>0xefff)Encoder_TIM=Encoder_TIM-0xffff; //转化计数值为有方向的值，大于0正转，小于0反转。
                                                          //TIM4->CNT范围为0-0xffff，初值为0。
    TIM4->CNT=0; //读取完后计数清零
    return Encoder_TIM; //返回值
}

/**************************************************************************
函数功能：TIM4中断服务函数
入口参数：无
返回  值：无
**************************************************************************/
void TIM4_IRQHandler(void)
{                                       
    if(TIM4->SR&0X0001)//溢出中断
    {                                                        
    }                   
    TIM4->SR&=~(1<<0);//清除中断标志位         
}

/**************************************************************************
函数功能：通用定时器2初始化函数，
入口参数：自动重装载值 预分频系数 默认定时时钟为72MHZ时，两者共同决定定时中断时间
返回  值：无
**************************************************************************/
void EncoderRead_TIM2(u16 arr, u16 psc)
{
    TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStrue; //定义一个定时中断的结构体
    NVIC_InitTypeDef NVIC_InitStrue; //定义一个中断优先级初始化的结构体

    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); //使能通用定时器2时钟

    TIM_TimeBaseInitStrue.TIM_Period=arr; //计数模式为向上计数时，定时器从0开始计数，计数超过到arr时触发定时中断服务函数
    TIM_TimeBaseInitStrue.TIM_Prescaler=psc; //预分频系数，决定每一个计数的时长
    TIM_TimeBaseInitStrue.TIM_CounterMode=TIM_CounterMode_Up; //计数模式：向上计数
    TIM_TimeBaseInitStrue.TIM_ClockDivision=TIM_CKD_DIV1; //一般不使用，默认TIM_CKD_DIV1
    TIM_TimeBaseInit(TIM2, &TIM_TimeBaseInitStrue); //根据TIM_TimeBaseInitStrue的参数初始化定时器TIM2

    TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE); //使能TIM2中断，中断模式为更新中断：TIM_IT_Update

    NVIC_InitStrue.NVIC_IRQChannel=TIM2_IRQn; //属于TIM2中断
    NVIC_InitStrue.NVIC_IRQChannelCmd=ENABLE; //中断使能
    NVIC_InitStrue.NVIC_IRQChannelPreemptionPriority=1; //抢占优先级为1级，值越小优先级越高，0级优先级最高
    NVIC_InitStrue.NVIC_IRQChannelSubPriority=1; //响应优先级为1级，值越小优先级越高，0级优先级最高
    NVIC_Init(&NVIC_InitStrue); //根据NVIC_InitStrue的参数初始化VIC寄存器，设置TIM2中断

    TIM_Cmd(TIM2, ENABLE); //使能定时器TIM2
}

/**************************************************************************
函数功能：TIM2中断服务函数 定时读取编码器数值并进行位置闭环控制 10ms进入一次
入口参数：无
返回  值：无
**************************************************************************/
void TIM2_IRQHandler()
{
  if(TIM_GetITStatus(TIM2, TIM_IT_Update)==1) //当发生中断时状态寄存器(TIMx_SR)的bit0会被硬件置1
    {
      Encoder=Read_Encoder();   //读取当前编码器读数，即速度
        CurrentPosition+=Encoder; //编码器读数(速度)积分得到位置

        if(MortorRun) //如果按键按下，运行电机控制程序
            {
                PWM=Position_FeedbackControl(TargetCircle, CurrentPosition); //位置闭环控制

                SetPWM(PWM); //设置PWM
            }
          else PWM=0,SetPWM(PWM); //如果按键再次按下，电机停止

        TIM_ClearITPendingBit(TIM2, TIM_IT_Update); //状态寄存器(TIMx_SR)的bit0置0
    }
}

/**************************************************************************
函数功能：速度闭环PID控制(实际为PI控制)
入口参数：目标速度 当前速度
返回  值：速度控制值
根据增量式离散PID公式 
ControlVelocity+=Kp[e（k）-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)]
e(k)代表本次偏差 
e(k-1)代表上一次的偏差  以此类推 
ControlVelocity代表增量输出
在我们的速度控制闭环系统里面，只使用PI控制
ControlVelocity+=Kp[e（k）-e(k-1)]+Ki*e(k)
**************************************************************************/
int Velocity_FeedbackControl(int TargetVelocity, int CurrentVelocity)
{
    int Bias;  //定义相关变量
    static int ControlVelocity, Last_bias; //静态变量，函数调用结束后其值依然存在

    Bias=TargetVelocity-CurrentVelocity; //求速度偏差

    ControlVelocity+=Velcity_Kp*(Bias-Last_bias)+Velcity_Ki*Bias;  //增量式PI控制器
    //Velcity_Kp*(Bias-Last_bias) 作用为限制加速度
    //Velcity_Ki*Bias             速度控制值由Bias不断积分得到 偏差越大加速度越大
    Last_bias=Bias;    
    return ControlVelocity; //返回速度控制值
}

/**************************************************************************
函数功能：位置式PID控制器
入口参数：目标圈数(位置) 当前位置
返回  值：速度控制值
根据位置式离散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_FeedbackControl(float Circle, int CurrentPosition)
{
    float TargetPosition,Bias, ControlVelocity;     //定义相关变量
    static float Last_bias, Integral_Bias;          //静态变量，函数调用结束后其值依然存在

    TargetPosition=Circle*1040*1.04; //目标位置=目标圈数*1040
    //10ms读取一次编码器(即100HZ)，电机减速比为20，霍尔编码器精度13，AB双相组合得到4倍频，
    //则转1圈编码器读数为20*13*4=1040，电机转速=Encoder*100/1040r/s 使用定时器2
    //1.04是误差系数，电机本身存在误差，可根据实际情况调整该系数以提高控制精度

    Bias=TargetPosition-CurrentPosition; //求位置偏差
    Integral_Bias+=Bias;
    if(Integral_Bias> 970) Integral_Bias= 970;    //积分限幅 防止到达目标位置后过冲
    if(Integral_Bias<-970) Integral_Bias=-970;    //积分限幅 防止到达目标位置后过冲
    ControlVelocity=Position_Kp*Bias+Position_Ki*Integral_Bias+Position_Kd*(Bias-Last_bias);  //增量式PI控制器
    Last_bias=Bias;    
    return ControlVelocity;    //返回速度控制值 
}

encoder.h

#ifndef __ENCODER_H 
#define __ENCODER_H 
#include "sys.h"
#include "TB6612.h"

void MotorEncoder_Init(void); 
int Read_Encoder(void);
void EncoderRead_TIM2(u16 arr, u16 psc);
int Position_FeedbackControl(float Circle, int CurrentVelocity);
int Velocity_FeedbackControl(int TargetVelocity, int CurrentVelocity);
int PWM_Restrict(int PWM_P, int TargetVelocity);
#endif

main.c

#include "delay.h"
#include "usart.h"            
#include "TB6612.h"
#include "motorencoder.h"
#include "usart.h"
#include "led.h"
#include "oled.h"
#include "key.h"

int   TargetVelocity=0, TargetCircle=10, CurrentPosition, Encoder, PWM;  //目标速度、目标圈数(位置)、编码器读数、PWM控制变量
float Velcity_Kp=20,  Velcity_Ki=5,  Velcity_Kd; //相关速度PID参数
float Position_Kp=120, Position_Ki=0.1, Position_Kd=400; //相关位置PID参数
int   MortorRun;  //允许电机控制标志位

/**************************************************************************
函数功能：OLED显示屏显示内容
入口参数：无
返回  值：无
**************************************************************************/
void Oled_Show(void)
{  
        OLED_Refresh_Gram(); //刷新显示屏
        OLED_ShowString(00,00,"V_P Feedback"); //速度闭环控制

      //显示目标位置，分正负
        OLED_ShowString(00,10,"Target_P :"); 
        if(TargetCircle>=0) 
        {
            OLED_ShowString(80,10,"+");
            OLED_ShowNumber(90,10,TargetCircle*1040*1.04,5,12); //1.04是误差系数，电机本身存在误差，可根据实际情况调整该系数以提高控制精度
        }
        else 
        {
            OLED_ShowString(80,10,"-");
            OLED_ShowNumber(90,10,-TargetCircle*1040*1.04,5,12); //1.04是误差系数，电机本身存在误差，可根据实际情况调整该系数以提高控制精度
        }

      //显示当前位置，分正负
        OLED_ShowString(00,20,"Current_P:"); 
        if(CurrentPosition>=0) 
        {
            OLED_ShowString(80,20,"+");
            OLED_ShowNumber(90,20,CurrentPosition,5,12);
        }
        else 
        {
            OLED_ShowString(80,20,"-");
            OLED_ShowNumber(90,20,-CurrentPosition,5,12);
        }

        //显示目标速度，分正负
        OLED_ShowString(00,30,"Target_V :");         
        if(TargetVelocity>=0)
        {
            OLED_ShowString(80,30,"+");
            OLED_ShowNumber(90,30,TargetVelocity,5,12);
        }
        else
        {
            OLED_ShowString(80,30,"-");
            OLED_ShowNumber(90,30,-TargetVelocity,5,12);
        }

        //显示当前速度，即编码器读数，分正负
        OLED_ShowString(00,40,"Current_V:");         
        if(Encoder>=0)
        {
            OLED_ShowString(80,40,"+");
            OLED_ShowNumber(90,40,Encoder,5,12);
        }
        else
        {
            OLED_ShowString(80,40,"-");
            OLED_ShowNumber(90,40,-Encoder,5,12);
        }

        //显示速度控制值，即PWM，分正负
        OLED_ShowString(00,50,"PWM      :");         
        if(PWM>=0)
        {
            OLED_ShowString(80,50,"+");
            OLED_ShowNumber(90,50,PWM,5,12);
        }
        else
        {
            OLED_ShowString(80,50,"-");
            OLED_ShowNumber(90,50,-PWM,5,12);
        }
}

/**************************************************************************
函数功能：主函数
入口参数：无
返回  值：无
**************************************************************************/
int main(void)
{
    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); //中断优先级分组
    delay_init();                //延迟函数初始化
    JTAG_Set(JTAG_SWD_DISABLE);  //关闭JTAG才能开启OLED显示屏 
    LED_Init();                  //LED灯初始化
    OLED_Init();                 //OLED显示屏初始化
    //uart_init(9600);           //串口初始化     
    MotorEncoder_Init();            //编码器初始化 使用定时器4
    TB6612_Init(7199, 0);        //电机驱动外设初始化 使用定时器3 
    EncoderRead_TIM2(7199, 99);  //10ms读取一次编码器(即100HZ)，电机减速比为20，霍尔编码器精度13，AB双相组合得到4倍频，
                              //则转1圈编码器读数为20*13*4=1040，电机转速=Encoder*100/1040r/s 使用定时器2
    delay_ms(2000);              //延迟等待初始化完成
    while(1)
    {        
        delay_ms(5);
        LED=0;    //LED灯闪烁
        if(KEY_Scan())MortorRun=!MortorRun; //按下按键MortorRun取反
        Oled_Show(); //OLED显示屏显示内容
    }
}

位置闭环控制

设定前 3 秒目标位置为 2 圈，则电机转(2-0)=2 圈；3 秒后目标位置为-1圈，则电机转(-1-2)=-3 圈。

在 TIM2 定时中断服务函数中添加一个静态变量 TimeCount，每次进入TIM2 定时中断服务函数 TimeCount 就加 1，而 TIM2 的的频率为 100HZ，即每10ms 进入一次 TIM2 定时中断服务函数，所以当 TimeCount=300 时即代表时间过去了 300*10ms=3s。

注意：目标速度 TargetVelocity 和目标圈数 TargetCircle 两个变量在该节中在定义时没有赋值，而是在 TIM2 定时中断服务函数中，允许电机运动了再赋值。

添加时间计数静态变量 TimeCount

static int TimeCount; //时间计数变量 静态变量，函数调用结束后其值依然存在

时间逻辑控制程序

TimeCount++;
if(TimeCount<=300)TargetCircle=2; //前 3 秒=300*10ms目标位置=2*1040*(正转 2 圈)
else if(300<TimeCount)TargetCircle=-1; //3 秒后 目标位置=-1*1040 -1-2=-3(反转 3 圈)
PWM=Position_FeedbackControl(TargetCircle, CurrentPosition);//位置闭环控制

实验操作

• 编码器初始化函数使能 GPIO、TIM 时钟→初始化 GPIO 引脚→初始化定时器→初始化编码器→清除相关标志位、数值→使能相关功能、定时器。
• 读取编码器计数值函数读取编码器计数并判断方向正反，读取完后将计数值清零。
• 定时读取编码器数值初始化函数使能TIM2时钟→初始化定时器→使能中断→初始化中断分组→使能定时器。
• TIM2 中断服务函数调用读取编码器计数值函数并进行位置闭环控制。

实验效果

接通电源打开电源开关后，连接电机和控制器，按下用户按键，电机将进行位置闭环控制：前 3 秒正转 2 圈，3 秒后反转 3 圈；旋转过程中再次按下按键电机停止。同时 OLED 显示屏会显示目标位置值(Target_P)、当前位置值(Current_P)目标速度值(Target_V)、读取速度值(Current_V)和当前 PWM 值。

Code

encoder.c

#include "motorencoder.h" 

//外部变量 extern说明改变量已在其它文件定义
extern int   Encoder, CurrentPosition; //当前速度、当前位置
extern int   TargetVelocity, TargetCircle, CurrentPosition, Encoder,PWM; //目标速度、目标圈数、编码器读数、PWM控制变量
extern float Velcity_Kp,  Velcity_Ki,  Velcity_Kd; //相关速度PID参数
extern float Position_Kp, Position_Ki, Position_Kd; //相关位置PID参数
extern int   MortorRun;  //允许电机控制标志位

/**************************************************************************
函数功能：编码器初始化函数
入口参数：无
返回  值：无
**************************************************************************/
void MotorEncoder_Init(void)
{
    GPIO_InitTypeDef GPIO_InitStructure; //定义一个引脚初始化的结构体  
    TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;//定义一个定时器初始化的结构体
    TIM_ICInitTypeDef TIM_ICInitStructure; //定义一个定时器编码器模式初始化的结构体

    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); //使能TIM4时钟
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); //使能CPIOB时钟

    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7;    //TIM4_CH1、TIM4_CH2
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; //浮空输入
    GPIO_Init(GPIOB, &GPIO_InitStructure);    //根据GPIO_InitStructure的参数初始化GPIO

    TIM_TimeBaseStructure.TIM_Period = 0xffff; //设定计数器自动重装值
    TIM_TimeBaseStructure.TIM_Prescaler = 0; // 预分频器 
    TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; //选择时钟分频：不分频
    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; //TIM向上计数模式
    TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure); //根据TIM_TimeBaseInitStruct的参数初始化定时器TIM4

    TIM_EncoderInterfaceConfig(TIM4, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising); //使用编码器模式3：CH1、CH2同时计数，为四分频
    TIM_ICStructInit(&TIM_ICInitStructure); //把TIM_ICInitStruct 中的每一个参数按缺省值填入
    TIM_ICInitStructure.TIM_ICFilter = 10;  //设置滤波器长度
    TIM_ICInit(TIM4, &TIM_ICInitStructure); //根TIM_ICInitStructure参数初始化定时器TIM4编码器模式

    TIM_ClearFlag(TIM4, TIM_FLAG_Update);//清除TIM的更新标志位
    TIM_ITConfig(TIM4, TIM_IT_Update, ENABLE); //更新中断使能
    TIM_SetCounter(TIM4,0); //初始化清空编码器数值

    TIM_Cmd(TIM4, ENABLE); //使能定时器4
}

/**************************************************************************
函数功能：读取TIM4编码器数值
入口参数：无
返回  值：无
**************************************************************************/
int Read_Encoder(void)
{
    int Encoder_TIM;
    Encoder_TIM=TIM4->CNT; //读取计数
    if(Encoder_TIM>0xefff)Encoder_TIM=Encoder_TIM-0xffff; //转化计数值为有方向的值，大于0正转，小于0反转。
                                                          //TIM4->CNT范围为0-0xffff，初值为0。
    TIM4->CNT=0; //读取完后计数清零
    return Encoder_TIM; //返回值
}

/**************************************************************************
函数功能：TIM4中断服务函数
入口参数：无
返回  值：无
**************************************************************************/
void TIM4_IRQHandler(void)
{                                       
    if(TIM4->SR&0X0001)//溢出中断
    {                                                        
    }                   
    TIM4->SR&=~(1<<0);//清除中断标志位         
}

/**************************************************************************
函数功能：通用定时器2初始化函数，
入口参数：自动重装载值 预分频系数 默认定时时钟为72MHZ时，两者共同决定定时中断时间
返回  值：无
**************************************************************************/
void EncoderRead_TIM2(u16 arr, u16 psc)
{
    TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStrue; //定义一个定时中断的结构体
    NVIC_InitTypeDef NVIC_InitStrue; //定义一个中断优先级初始化的结构体

    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); //使能通用定时器2时钟

    TIM_TimeBaseInitStrue.TIM_Period=arr; //计数模式为向上计数时，定时器从0开始计数，计数超过到arr时触发定时中断服务函数
    TIM_TimeBaseInitStrue.TIM_Prescaler=psc; //预分频系数，决定每一个计数的时长
    TIM_TimeBaseInitStrue.TIM_CounterMode=TIM_CounterMode_Up; //计数模式：向上计数
    TIM_TimeBaseInitStrue.TIM_ClockDivision=TIM_CKD_DIV1; //一般不使用，默认TIM_CKD_DIV1
    TIM_TimeBaseInit(TIM2, &TIM_TimeBaseInitStrue); //根据TIM_TimeBaseInitStrue的参数初始化定时器TIM2

    TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE); //使能TIM2中断，中断模式为更新中断：TIM_IT_Update
    NVIC_InitStrue.NVIC_IRQChannel=TIM2_IRQn; //属于TIM2中断
    NVIC_InitStrue.NVIC_IRQChannelCmd=ENABLE; //中断使能
    NVIC_InitStrue.NVIC_IRQChannelPreemptionPriority=1; //抢占优先级为1级，值越小优先级越高，0级优先级最高
    NVIC_InitStrue.NVIC_IRQChannelSubPriority=1; //响应优先级为1级，值越小优先级越高，0级优先级最高
    NVIC_Init(&NVIC_InitStrue); //根据NVIC_InitStrue的参数初始化VIC寄存器，设置TIM2中断
    TIM_Cmd(TIM2, ENABLE); //使能定时器TIM2
}

/**************************************************************************
函数功能：TIM2中断服务函数 定时读取编码器数值并进行位置闭环控制 10ms进入一次
入口参数：无
返回  值：无
**************************************************************************/
void TIM2_IRQHandler()
{
    static int TimeCount; //时间计数变量 静态变量，函数调用结束后其值依然存在
  if(TIM_GetITStatus(TIM2, TIM_IT_Update)==1) //当发生中断时状态寄存器(TIMx_SR)的bit0会被硬件置1
    {
      Encoder=Read_Encoder();   //读取当前编码器读数，即速度
        CurrentPosition+=Encoder; //编码器读数(速度)积分得到位置

        if(MortorRun) //如果按键按下，运行电机控制程序
            {
                TimeCount++;
                if(TimeCount<=300)TargetCircle=2;      //前3秒=300*10ms  目标位置=2*1040*(正转2圈)
                else if(300<TimeCount)TargetCircle=-1; //3秒后  目标位置=-1*1040 -1-2=-3(反转3圈)
                PWM=Position_FeedbackControl(TargetCircle, CurrentPosition); //位置闭环控制
                SetPWM(PWM); //设置PWM
            }
          else PWM=0,SetPWM(PWM); //如果按键再次按下，电机停止

        TIM_ClearITPendingBit(TIM2, TIM_IT_Update); //状态寄存器(TIMx_SR)的bit0置0
    }
}

/**************************************************************************
函数功能：速度闭环PID控制(实际为PI控制)
入口参数：目标速度 当前速度
返回  值：速度控制值
根据增量式离散PID公式 
ControlVelocity+=Kp[e（k）-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)]
e(k)代表本次偏差 
e(k-1)代表上一次的偏差  以此类推 
ControlVelocity代表增量输出
在我们的速度控制闭环系统里面，只使用PI控制
ControlVelocity+=Kp[e（k）-e(k-1)]+Ki*e(k)
**************************************************************************/
int Velocity_FeedbackControl(int TargetVelocity, int CurrentVelocity)
{
    int Bias;  //定义相关变量
    static int ControlVelocity, Last_bias; //静态变量，函数调用结束后其值依然存在
    Bias=TargetVelocity-CurrentVelocity; //求速度偏差
    ControlVelocity+=Velcity_Kp*(Bias-Last_bias)+Velcity_Ki*Bias;  //增量式PI控制器
    Last_bias=Bias;    
    return ControlVelocity; //返回速度控制值
}

/**************************************************************************
函数功能：位置式PID控制器
入口参数：目标圈数(位置) 当前位置
返回  值：速度控制值
根据位置式离散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_FeedbackControl(float Circle, int CurrentPosition)
{
    float TargetPosition,Bias, ControlVelocity;     //定义相关变量
    static float Last_bias, Integral_Bias;          //静态变量，函数调用结束后其值依然存在

    TargetPosition=Circle*1040*1.04; //目标位置=目标圈数*1040
    //10ms读取一次编码器(即100HZ)，电机减速比为20，霍尔编码器精度13，AB双相组合得到4倍频，
    //则转1圈编码器读数为20*13*4=1040，电机转速=Encoder*100/1040r/s 使用定时器2
    //1.04是误差系数，电机本身存在误差，可根据实际情况调整该系数以提高控制精度

    Bias=TargetPosition-CurrentPosition; //求位置偏差
    Integral_Bias+=Bias;
    if(Integral_Bias> 970) Integral_Bias= 970;    //积分限幅 防止到达目标位置后过冲
    if(Integral_Bias<-970) Integral_Bias=-970;    //积分限幅 防止到达目标位置后过冲

    ControlVelocity=Position_Kp*Bias+Position_Ki*Integral_Bias+Position_Kd*(Bias-Last_bias);  //增量式PI控制器
    Last_bias=Bias;    
    return ControlVelocity;    //返回速度控制值 
}

encoder.h

#ifndef __ENCODER_H 
#define __ENCODER_H 
#include "sys.h"
#include "TB6612.h"

void MotorEncoder_Init(void); 
int Read_Encoder(void);
void EncoderRead_TIM2(u16 arr, u16 psc);
int Position_FeedbackControl(float Circle, int CurrentVelocity);
int Velocity_FeedbackControl(int TargetVelocity, int CurrentVelocity);
int PWM_Restrict(int PWM_P, int TargetVelocity);
#endif

main.c

#include "delay.h"
#include "usart.h"            
#include "TB6612.h"
#include "motorencoder.h"
#include "usart.h"
#include "led.h"
#include "oled.h"
#include "key.h"

int   TargetVelocity=0, TargetCircle=0, CurrentPosition, Encoder, PWM;  //目标速度、目标圈数(位置)、编码器读数、PWM控制变量
float Velcity_Kp=20,  Velcity_Ki=5,  Velcity_Kd; //相关速度PID参数
float Position_Kp=120, Position_Ki=0.1, Position_Kd=400; //相关位置PID参数
int   MortorRun;  //允许电机控制标志位

/**************************************************************************
函数功能：OLED显示屏显示内容
入口参数：无
返回  值：无
**************************************************************************/
void Oled_Show(void)
{  
        OLED_Refresh_Gram(); //刷新显示屏
        OLED_ShowString(00,00,"V_P Feedback"); //速度闭环控制

      //显示目标位置，分正负
        OLED_ShowString(00,10,"Target_P :"); 
        if(TargetCircle>=0) 
        {
            OLED_ShowString(80,10,"+");
            OLED_ShowNumber(90,10,TargetCircle*1040*1.04,5,12); //1.04是误差系数，电机本身存在误差，可根据实际情况调整该系数以提高控制精度
        }
        else 
        {
            OLED_ShowString(80,10,"-");
            OLED_ShowNumber(90,10,-TargetCircle*1040*1.04,5,12); //1.04是误差系数，电机本身存在误差，可根据实际情况调整该系数以提高控制精度
        }

      //显示当前位置，分正负
        OLED_ShowString(00,20,"Current_P:"); 
        if(CurrentPosition>=0) 
        {
            OLED_ShowString(80,20,"+");
            OLED_ShowNumber(90,20,CurrentPosition,5,12);
        }
        else 
        {
            OLED_ShowString(80,20,"-");
            OLED_ShowNumber(90,20,-CurrentPosition,5,12);
        }

        //显示目标速度，分正负
        OLED_ShowString(00,30,"Target_V :");         
        if(TargetVelocity>=0)
        {
            OLED_ShowString(80,30,"+");
            OLED_ShowNumber(90,30,TargetVelocity,5,12);
        }
        else
        {
            OLED_ShowString(80,30,"-");
            OLED_ShowNumber(90,30,-TargetVelocity,5,12);
        }

        //显示当前速度，即编码器读数，分正负
        OLED_ShowString(00,40,"Current_V:");         
        if(Encoder>=0)
        {
            OLED_ShowString(80,40,"+");
            OLED_ShowNumber(90,40,Encoder,5,12);
        }
        else
        {
            OLED_ShowString(80,40,"-");
            OLED_ShowNumber(90,40,-Encoder,5,12);
        }

        //显示速度控制值，即PWM，分正负
        OLED_ShowString(00,50,"PWM      :");         
        if(PWM>=0)
        {
            OLED_ShowString(80,50,"+");
            OLED_ShowNumber(90,50,PWM,5,12);
        }
        else
        {
            OLED_ShowString(80,50,"-");
            OLED_ShowNumber(90,50,-PWM,5,12);
        }
}

/**************************************************************************
函数功能：主函数
入口参数：无
返回  值：无
**************************************************************************/
int main(void)
{
    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); //中断优先级分组
    delay_init();                //延迟函数初始化
    JTAG_Set(JTAG_SWD_DISABLE);  //关闭JTAG才能开启OLED显示屏 
    LED_Init();                  //LED灯初始化
    OLED_Init();                 //OLED显示屏初始化
    MotorEncoder_Init();            //编码器初始化 使用定时器4
    TB6612_Init(7199, 0);        //电机驱动外设初始化 使用定时器3 
    EncoderRead_TIM2(7199, 99);  //10ms读取一次编码器(即100HZ)，电机减速比为20，霍尔编码器精度13，AB双相组合得到4倍频，
                          //则转1圈编码器读数为20*13*4=1040，电机转速=Encoder*100/1040r/s 使用定时器2
    delay_ms(2000);              //延迟等待初始化完成
    while(1)
    {        
        delay_ms(5);
        LED=0;    //LED灯闪烁
        if(KEY_Scan())MortorRun=!MortorRun; //按下按键MortorRun取反                    
        Oled_Show(); //OLED显示屏显示内容
    }
}