PID例子

［電機與驅動］ 一個簡單的PID

PID

#include 《hidef.h》 /* common defines and macros */

#include “derivative.h” /* derivative-specific definitions */

#include 《mc9s12xs128.h》

//定義PID參數

#define VV_KPVALUE 3 //比例

#define VV_KIVALUE 40 //積分

#define VV_KDVALUE 3 //微分

#define VV_MAX 10000 //返回的最大值，是pwm的周期值

#define VV_MIN 0

#define VV_DEADLINE 0X08 //速度PID，設置死區范圍

typedef struct PID //定義數法核心數據

{

signed int vi_Ref; //速度PID，速度設定值

signed int vi_FeedBack; //速度PID，速度反饋值

signed long vi_PreError; //速度PID，前一次，速度誤差，，vi_Ref - vi_FeedBack

signed long vi_PreDerror; //速度PID，前一次，速度誤差之差，d_error-PreDerror;

unsigned int v_Kp; //速度PID，Ka = Kp

unsigned int v_Ki; //速度PID，Kb = Kp * （ T / Ti ）

unsigned int v_Kd; //速度PID，

signed long vl_PreU; //電機控制輸出值

}PID;

PID sPID; // PID Control Structure

void PIDInit（void）

{

sPID.vi_Ref = 0 ; //速度設定值

sPID.vi_FeedBack = 0 ; //速度反饋值

sPID.vi_PreError = 0 ; //前一次，速度誤差，，vi_Ref - vi_FeedBack

sPID.vi_PreDerror = 0 ; //前一次，速度誤差之差，d_error-PreDerror;

sPID.v_Kp = VV_KPVALUE;

sPID.v_Ki = VV_KIVALUE;

sPID.v_Kd = VV_KDVALUE;

sPID.vl_PreU = 0 ; //電機控制輸出值

}

unsigned int v_PIDCalc（ PID *pp ）

{

signed long error，d_error，dd_error;

error = （signed long）（pp-》vi_Ref - pp-》vi_FeedBack）; // 偏差計算

d_error = error - pp-》vi_PreError;

dd_error = d_error - pp-》vi_PreDerror;

pp-》vi_PreError = error; //存儲當前偏差

pp-》vi_PreDerror = d_error;

if（ （ error 《 VV_DEADLINE ） && （ error 》 -VV_DEADLINE ） ）; //設置調節死區

//速度PID計算

pp-》vl_PreU += （signed long）（ pp -》 v_Kp * d_error + pp -》 v_Ki * error + pp-》v_Kd*dd_error）;

else if（ pp-》vl_PreU 》= VV_MAX ） //速度PID，防止調節最高溢出

pp-》vl_PreU = VV_MAX;

else if（ pp-》vl_PreU 《= VV_MIN ） //速度PID，防止調節最低溢出

pp-》vl_PreU = VV_MIN;

else

;

return （ pp-》vl_PreU ）; // 返回預調節占空比

}

void main（void） {

/* put your own code here */

InitMCu（）;

IncPIDInit（）;

int g_CurrentVelocity=0; //全局變量也初始化

int g_Flag=0; //全局變量也初始化

EnableInterrupts;

While（1）

{

if （g_Flag&&vi_FeedBack）

{

PWMOUT+= v_PIDCalc（ PID *pp ）;

g_Flag&=~ vi_FeedBack;

}

}

}

for（;;） {

_FEED_COP（）; /* feeds the dog */

} /* loop forever */

/* please make sure that you never leave main */

}