STM32 PID精准控制电机转到指定位置源程序

ID:519988 · 发表于 2019-4-23 21:43

步进电机转到指定位置

单片机源程序如下:

#include "led.h"
#include "delay.h"
#include "key.h"
#include "sys.h"
#include "beep.h"
#include "encoder.h"
#include "timer.h"
int main(void)
{
vu8 key=0;
delay_init(); //延时函数初始化
uart_init(9600); //=====串口初始化
Encoder_Init_TIM2(); //=====编码器接口
TIM6_Int_Init(99,7199); //10MS进入一次中断
TIM3_PWM_Init(7199,0); //不分频。PWM频率=72000000/900=80Khz
while(1)
{
key=KEY_Scan(0); //得到键值
if(key)
{
switch(key)
{
case WKUP_PRES: //控制蜂鸣器
LED0=!LED0;
break;
case KEY2_PRES: //控制LED0翻转
LED0=!LED0;
break;
case KEY1_PRES: //控制LED1翻转
LED1=!LED1;
break;
case KEY0_PRES: //同时控制LED0,LED1翻转
LED0=!LED0;
LED1=!LED1;
break;
}
}else delay_ms(10);
}
}

复制代码

#include "control.h"
#define PI 3.141592653
#include "sys.h"
int Encoder,Target_position=10280; //Encoder编码器的脉冲计数，相当于实际速度 Target_velocity目标速度一周1040脉冲初始位置10000在encoder.c中设置
int Moto; //电机PWM变量应是Motor的向Moto致敬
void TIM6_IRQHandler(void) //TIM6中断函数实现了编码器角速度线速度的计算 10MS进入一次中断
{
if (TIM_GetITStatus(TIM6, TIM_IT_Update) != RESET) //检查TIM3更新中断发生与否
{
TIM_ClearITPendingBit(TIM6, TIM_IT_Update ); //清除TIMx更新中断标志
Encoder=Read_Encoder(2);
printf("位置%d\r\n",Encoder);
Moto=Position_PID(Encoder,Target_position); //===位置PID控制器
Xianfu_Pwm(); //===PWM限幅
Set_Pwm(Moto); //===赋值给PWM寄存器
}
}
/**************************************************************************
函数功能：赋值给PWM寄存器
入口参数：PWM
返回值：无
**************************************************************************/
void Set_Pwm(int moto1)
{
if(moto1<0) IN2=1, IN1=0; //换向引脚
else IN2=0, IN1=1;
TIM_SetCompare2(TIM3,myabs(moto1));
}
/**************************************************************************
函数功能：限制PWM赋值
入口参数：无
返回值：无
**************************************************************************/
void Xianfu_Pwm(void)
{
int Amplitude=5000; //===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 Position_PID (int Encoder,int Target)
{
float Position_KP=10,Position_KI=0.1,Position_KD=40;
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; //增量输出
}