STM32F429使用位置式PID算法控制电机转速遇到问题

胖胖胖胖啊 · 发表于 2019-7-25 11:17:06

这几天在调试PID，想用来控制电机速度，用到PWM、定时器编码器模式、带AB相编码盘的直流减速电机；程序是参考某位大佬的，移植过来F429开发板，电机能跑，但是PIM没作用到。由于自己水平还很低，现在还在想哪里出了问题，想着人多力量大，求助一下论坛大佬们，给些建议。下面就是我移植过来的代码：

定时器输出PWM主要代码：

/*****************************************************************************************************************
函数功能：实现对定时器的初始化，所用到的定时器为定时器3，通道1和通道2，分别输出PWM波控制电机
实现编码器对电机速度的获取，并打印到串口助手上
电机GPIO： TIM3_CH1Handler --> PB4 TIM3_CH2Handler --> PB5
占空比： compare1：TIM3通道1的占空比，compare2：TIM3通道2的占空比
相关解析：输出的方波周期就是自动重装载值的值（us为单位），占空比 = compare / arr
A4950接线：单片机 A4950模块
5V --> VCC
GND --> GND
PB4 --> AN1
GND --> AN2
PB5 --> BN1
GND --> BN2
AOUT1 --> 电机1+
AOUT2 --> 电机1-
BOUT1 --> 电机2+
BOUT2 --> 电机2-
VM----->>>>> 外接电源，7.6V---12V，需要共地
*****************************************************************************************************************/
#include "timer.h"
#include "led.h"
#include "encoder.h"
TIM_HandleTypeDef TIM3_PWM_Handler; //定时器3句柄
TIM_OC_InitTypeDef TIM3_CH1Handler; //定时器3通道1句柄
TIM_OC_InitTypeDef TIM3_CH2Handler; //定时器3通道2句柄
//PWM 输出初始化
//arr：自动重装值 psc：时钟预分频数
void TIM3_PWM_Init(u16 arr,u16 psc)
{
TIM3_PWM_Handler.Instance=TIM3;//定时器3
TIM3_PWM_Handler.Init.Prescaler= psc;//时钟预分频系数
TIM3_PWM_Handler.Init.CounterMode=TIM_COUNTERMODE_UP;//向上计数模式
TIM3_PWM_Handler.Init.Period=arr;//自动重装载值
TIM3_PWM_Handler.Init.ClockDivision=TIM_CLOCKDIVISION_DIV1;//时钟分频因子
HAL_TIM_PWM_Init(&TIM3_PWM_Handler);
TIM3_CH1Handler.OCMode=TIM_OCMODE_PWM1;//PWM1模式
TIM3_CH1Handler.Pulse=arr/2;//设置比较值,此值用来确定占空比
TIM3_CH1Handler.OCPolarity=TIM_OCPOLARITY_LOW;//输出比较极性低
HAL_TIM_PWM_ConfigChannel(&TIM3_PWM_Handler,&TIM3_CH1Handler,TIM_CHANNEL_1);//配置 TIM3通道1
HAL_TIM_PWM_ConfigChannel(&TIM3_PWM_Handler,&TIM3_CH1Handler,TIM_CHANNEL_2);//配置 TIM3通道2
HAL_TIM_PWM_Start(&TIM3_PWM_Handler,TIM_CHANNEL_1);//开启PWM通道1
HAL_TIM_PWM_Start(&TIM3_PWM_Handler,TIM_CHANNEL_2);//开启PWM通道2
}
//定时器底层驱动，时钟使能，引脚配置
//此函数会被 HAL_TIM_PWM_Init()调用
//htim:定时器句柄
void HAL_TIM_PWM_MspInit(TIM_HandleTypeDef *htim)
{
GPIO_InitTypeDef GPIO_Initure;
__HAL_RCC_TIM3_CLK_ENABLE();//开启TIM3时钟
__HAL_RCC_GPIOB_CLK_ENABLE();//开启GPIOB时钟
GPIO_Initure.Pin=GPIO_PIN_4 | GPIO_PIN_5;//PB4和PB5
GPIO_Initure.Mode=GPIO_MODE_AF_PP;//复用为推挽输出
GPIO_Initure.Pull=GPIO_PULLUP;//上拉
GPIO_Initure.Speed=GPIO_SPEED_HIGH;//高速
GPIO_Initure.Alternate=GPIO_AF2_TIM3; //PB 复用为 TIM3_CH1
HAL_GPIO_Init(GPIOB,&GPIO_Initure);
}
extern long Pulse_Num;
extern float Speed_num;
u8 TimerFlag=0;
void TIM3_IRQHandler(void)
{
if(__HAL_TIM_GET_FLAG(&TIM3_PWM_Handler, TIM_FLAG_UPDATE) != RESET)//溢出中断
{
Speed_num = Pulse_Num;
Pulse_Num = 0;
TimerFlag=1;
}
__HAL_TIM_CLEAR_FLAG(&TIM3_PWM_Handler, TIM_FLAG_UPDATE);//清除中断标志位
}
//设置TIM3通道1的占空比
//compare1:比较值
void TIM_SetTIM3Compare1(u32 compare1)
{
TIM3->CCR1=compare1;
}
//设置TIM3通道2的占空比
//compare2:比较值
void TIM_SetTIM3Compare2(u32 compare2)
{
TIM3->CCR2=compare2;
}

复制代码

定时器编码器模式主要代码：

/*********************************************************************************************************
函数功能：定时器4编码器模式检测电机转速
测速GPIO： A相 --> PD12(TIM4_CH1) B相 --> PD13(TIM4_CH2)
电机脉冲：编码器AB相输出：每转每相各输出360个脉冲
*********************************************************************************************************/
#include "encoder.h"
#include "timer.h"
#include "led.h"
long Interupt_Num=0 ;
long Pulse_Num =0 ;
float Circle_NUm =0;
float Speed_num=0;
TIM_HandleTypeDef TIM4_Handler;//定时器4句柄
TIM_Encoder_InitTypeDef TIM4_Encoder_Handler;//定时器4编码器模式句柄
//编码器模式初始化函数
void TIM4_Encoder_Init(u16 arr, u16 psc)
{
TIM4_Handler.Instance=TIM4;//选择定时器4
TIM4_Handler.Init.Prescaler=psc;//时钟预分频系数
TIM4_Handler.Init.CounterMode=TIM_COUNTERMODE_UP;//向上计数
TIM4_Handler.Init.Period=arr;//自动装载值
TIM4_Handler.Init.ClockDivision=TIM_CLOCKDIVISION_DIV1;//时钟分频因子
//HAL_TIM_Base_Init(&TIM4_Handler);
TIM4_Encoder_Handler.EncoderMode=TIM_ENCODERMODE_TI12;
TIM4_Encoder_Handler.IC1Filter=0;
TIM4_Encoder_Handler.IC1Polarity=TIM_ICPOLARITY_RISING;
TIM4_Encoder_Handler.IC1Prescaler=TIM_ICPSC_DIV1;
TIM4_Encoder_Handler.IC1Selection=TIM_ICSELECTION_DIRECTTI;
TIM4_Encoder_Handler.IC2Filter=0;
TIM4_Encoder_Handler.IC2Polarity=TIM_ICPOLARITY_RISING;
TIM4_Encoder_Handler.IC2Prescaler=TIM_ICPSC_DIV1;
TIM4_Encoder_Handler.IC2Selection=TIM_ICSELECTION_DIRECTTI;
HAL_TIM_Encoder_Init(&TIM4_Handler, &TIM4_Encoder_Handler);
HAL_TIM_Encoder_Start(&TIM4_Handler, TIM_CHANNEL_ALL);//开启编码器中断
HAL_TIM_Encoder_Start_IT(&TIM4_Handler, TIM_CHANNEL_ALL);//开启更新中断
TIM4->CNT=0;
__HAL_TIM_ENABLE(&TIM4_Handler);//使能中断
__HAL_TIM_ENABLE_IT(&TIM4_Handler, TIM_IT_UPDATE);//使能更新中断
}
//编码器模式回调函数
void HAL_TIM_Encoder_MspInit(TIM_HandleTypeDef *htim)
{
GPIO_InitTypeDef GPIOD_Initure;
__HAL_RCC_GPIOD_CLK_ENABLE();
__HAL_RCC_TIM4_CLK_ENABLE();
GPIOD_Initure.Pin=GPIO_PIN_12 | GPIO_PIN_13;
GPIOD_Initure.Mode=GPIO_MODE_AF_PP;
GPIOD_Initure.Pull=GPIO_PULLUP;
GPIOD_Initure.Speed=GPIO_SPEED_HIGH;
GPIOD_Initure.Alternate=GPIO_AF2_TIM4;//PD复用为TIM4模式
HAL_GPIO_Init(GPIOD, &GPIOD_Initure);
HAL_NVIC_SetPriority(TIM4_IRQn,1,3); //设置中断优先级，抢占优先级1，子优先级3
HAL_NVIC_EnableIRQ(TIM4_IRQn); //开启ITM4中断
HAL_TIM_Base_Start_IT(&TIM4_Handler); //使能定时器4和定时器4更新中断：TIM_IT_UPDATE
}
void TIM4_IRQHandler(void)
{
HAL_TIM_IRQHandler(&TIM4_Handler);
}
//回调函数，定时器中断服务函数调用
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
if(htim==(&TIM4_Handler))
{
LED1 = !LED1 ;
if ( (TIM4->CR1&0x0010) == 0 ) //向上计数溢出
{
Interupt_Num++ ;
}
else //向下计数溢出
{
Interupt_Num-- ;
}
}
}
//输出编码器获取值
void Circle_Print()
{
if(Interupt_Num ==0 ) //CNT从0开始，刚开始的时候计算脉冲的方法
{
Pulse_Num = TIM4->CNT;
}
else
{
Pulse_Num = Interupt_Num*2000 +TIM4->CNT; //在第一圈完成之后，计算脉冲数多的方法
}
//Circle_NUm=(float)Pulse_Num/360;
printf("Interupt_Num的值是:%ld , 捕获到的总数是:%ld\r\n" , Interupt_Num, Pulse_Num);
}

复制代码

PID算法代码：

/*************************************************************************************************
函数功能：位置式PID控制算法的实现
位置公式：经离散化处理后，PIDout = Kp * e(k) + Ki * ∑(0-->k）e(j) + Kd * [e(k) - e(k-1)],
Ki = Kp*T/Ti,Kd = Kp*Td/T， Kp为比例系数，Ki为积分系数，Kd为微分系数
增量公式：经过离散化处理后：PIDout = Kp[e(k) - e(k-1)] + Ki * e(k) + Kd[e(k) - 2e(k-1) + e(k-2)]
e(k)为本次偏差，e(k-1)为上一次偏差，e(k-2)为上上次偏差
偏差值：输出值与设定值之间的差值，用e(k)表示，即：e(k) = S(k) - O(k)
离散化：在不改变数据的相对大小的条件下，对数据进行相应的缩小；离散情况下积分其实就是累加过程
速度闭环：速度闭环控制就是根据单位时间获取的脉冲数测量电机的速度信息，并与目标值进行比较，得到控制偏差，
然后通过对偏差的比例、积分、微分进行控制，使偏差趋向于0的过程
参考： https://blog.csdn.net/qq_25352981/article/details/81007075
https://blog.csdn.net/kilotwo/article/details/79952530
https://www.amobbs.com/thread-5043342-1-1.html
*************************************************************************************************/
#include "pid.h"
#include "encoder.h"
#include "timer.h"
void PID_Init(PID_TypeDef * PID)
{
PID->errorSumDownLim = -188;
PID->errorSumUpLim = 188;
PID->PIDoutDownLim = -999;
PID->PIDoutUpLim = 999;
PID->errorUpLim = 50;
PID->errorDownLim = -50;
PID->delErrorUpLim = 20; //试凑值
PID->delErrorDownLim = -40;
}
//将PID的Kp、Ki、Kd系数简化用x、y、z表示
void PID_SetParameter(PID_TypeDef * PID,float x,float y,float z)
{
PID->kp=x;
PID->ki=y;
PID->kd=z;
}
//简化设定值表示
void PID_SetTargetVal(PID_TypeDef * PID, float x)
{
PID->TargetVal = x;
}
//简化输出值表示
void PID_SetNowVal(PID_TypeDef * PID, float x)
{
PID->NowVal = x;
}
//PID计算函数
void PID_Clac(PID_TypeDef * PID)
{
//得到偏差，P项的值
PID->error = PID->TargetVal - PID->NowVal; //偏差 = 设定 - 当前
if(PID->error > PID->errorUpLim) //误差限幅
{
PID->error = PID->errorUpLim;
}
else if(PID->error < PID->errorDownLim)
{
PID->error = PID->errorDownLim;
}
//得到偏差之和，I项的值
PID->errorSum += PID->error; //偏差求和
if(PID->errorSum > PID->errorSumUpLim) //偏差和限幅
{
PID->errorSum = PID->errorSumUpLim;
}
else if(PID->errorSum < PID->errorSumDownLim)
{
PID->errorSum = PID->errorSumDownLim;
}
//得到最近两次偏差之差，D项的值
PID->delError = PID->error - PID->error1; //偏差微分
PID->error1 = PID->error; //把本次偏差赋值给上次偏差
if(PID->delError > PID->delErrorUpLim) //偏差微分限幅
{
PID->delError = PID->delErrorUpLim;
}
else if(PID->delError < PID->delErrorDownLim)
{
PID->delError = PID->delErrorDownLim;
}
//PID三项分别输出
PID->Pout = PID->kp * PID->error; //比例输出
PID->Iout = PID->ki * PID->errorSum; //积分输出
PID->Dout = PID->kd * PID->delError; //微分输出
//得到PID项之和，为位置式PID控制算法
PID->PIDout = PID->Pout + PID->Iout + PID->Dout; //PID输出
if(PID->PIDout > PID->PIDoutUpLim) //PID输出限幅
{
PID->PIDout = PID->PIDoutUpLim;
}
else if(PID->PIDout < PID->PIDoutDownLim)
{
PID->PIDout = PID->PIDoutDownLim;
}
}