STM32 PID精準(zhǔn)控制電機(jī)轉(zhuǎn)到指定位置源程序

ID:519988 · 發(fā)表于 2019-4-23 21:43

步進(jìn)電機(jī)轉(zhuǎn)到指定位置

單片機(jī)源程序如下:

#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(); //延時函數(shù)初始化
uart_init(9600); //=====串口初始化
Encoder_Init_TIM2(); //=====編碼器接口
TIM6_Int_Init(99,7199); //10MS進(jìn)入一次中斷
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翻轉(zhuǎn)
LED0=!LED0;
break;
case KEY1_PRES: //控制LED1翻轉(zhuǎn)
LED1=!LED1;
break;
case KEY0_PRES: //同時控制LED0,LED1翻轉(zhuǎn)
LED0=!LED0;
LED1=!LED1;
break;
}
}else delay_ms(10);
}
}

復(fù)制代碼

#include "control.h"
#define PI 3.141592653
#include "sys.h"
int Encoder,Target_position=10280; //Encoder編碼器的脈沖計數(shù)，相當(dāng)于實際速度 Target_velocity目標(biāo)速度一周1040脈沖初始位置10000在encoder.c中設(shè)置
int Moto; //電機(jī)PWM變量應(yīng)是Motor的向Moto致敬
void TIM6_IRQHandler(void) //TIM6中斷函數(shù)實現(xiàn)了編碼器角速度線速度的計算 10MS進(jìn)入一次中斷
{
if (TIM_GetITStatus(TIM6, TIM_IT_Update) != RESET) //檢查TIM3更新中斷發(fā)生與否
{
TIM_ClearITPendingBit(TIM6, TIM_IT_Update ); //清除TIMx更新中斷標(biāo)志
Encoder=Read_Encoder(2);
printf("位置%d\r\n",Encoder);
Moto=Position_PID(Encoder,Target_position); //===位置PID控制器
Xianfu_Pwm(); //===PWM限幅
Set_Pwm(Moto); //===賦值給PWM寄存器
}
}
/**************************************************************************
函數(shù)功能：賦值給PWM寄存器
入口參數(shù)：PWM
返回值：無
**************************************************************************/
void Set_Pwm(int moto1)
{
if(moto1<0) IN2=1, IN1=0; //換向引腳
else IN2=0, IN1=1;
TIM_SetCompare2(TIM3,myabs(moto1));
}
/**************************************************************************
函數(shù)功能：限制PWM賦值
入口參數(shù)：無
返回值：無
**************************************************************************/
void Xianfu_Pwm(void)
{
int Amplitude=5000; //===PWM滿幅是7200 限制在7100
if(Moto<-Amplitude) Moto=-Amplitude;
if(Moto>Amplitude) Moto=Amplitude;
}
/**************************************************************************
函數(shù)功能：絕對值函數(shù)
入口參數(shù)：int
返回值：unsigned int
**************************************************************************/
int myabs(int a)
{
int temp;
if(a<0) temp=-a;
else temp=a;
return temp;
}
/**************************************************************************
函數(shù)功能：增量PI控制器
入口參數(shù)：編碼器測量值，目標(biāo)速度
返回值：電機(jī)PWM
根據(jù)增量式離散]]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代表增量輸出
在我們的速度控制閉環(huán)系統(tǒng)里面，只使用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; //增量輸出
}