简易旋转倒立摆PID算法_旋转倒立摆pid控制

简易旋转倒立摆PID算法由刀豆文库小编整理，希望给你工作、学习、生活带来方便，猜你可能喜欢“旋转倒立摆pid控制”。

C语言PID算法

简易旋转倒立摆控制参考程序 #include #include

typedef struct PID { /*K1=实际放大倍数,T1=实际积分时间,T2=实际微分时

间,T=采样周期*/

double

SetPoint;

/*定义PID结构体*/

double

K1;

double

T1;

double

T2;

double

T;

double

Err1;

/*前一时刻误差,E(K-1)*/

} PID;

double PIDCalc(PID *pp, double NextPoint,double Ud1,double Ui1)

/*PID计算 */

{

double Ti,Td,Kp,Ki,Kd,Ud,Up,Ui,Err;

Ti =pp-> T1 + pp-> T2;

/*积分时间*/

Td =(pp-> T1 * pp-> T2)/(pp-> T1 + pp-> T2);

/*微分时间*/

Kp =pp-> K1 *((pp-> T1 + pp-> T2)/ pp-> T1);

/*比例系数*/

Ki=pp-> T / Ti * Kp;

/*积分系数*/

Kd = Td / pp-> T * Kp;

/*微分系数*/

Err = pp-> SetPoint-NextPoint;

/*当前误差*/

Ud = pp->T2/((Kd * pp-> T)+ pp-> T2)* Ud1+ Kd *(pp-> T2 + pp-> T)/(Kd * pp-> T + pp-> T2)* Err-Kd * pp-> T2 /(Kd * pp-> T + pp-> T2)*pp-> Err1;

/*微分 作用*/

Ui = Ui1 + pp-> K1 *(pp-> T / pp-> T1)* Ud;/*积分作用*/

Up = pp-> K1 * Ud;

/*比例作用*/

Ud1=Ud;

/*UD1=ud(k-1),ui1=ui(k-1)*/

Ui1=Ui;

return(Ud + Up + Ui);

/*y(k)*/ }

void PIDInit(PID *pp){

memset(pp,0,sizeof(PID));

}

/*double sensor(void)

{

return 1.0;}*/

/*输入口*/

void actuator(double rDelta,double LastrDelta)

/*输出口*/

{

double n;

n=rDelta-LastrDelta;

/*y(k)-y(k-1)*/

LastrDelta=rDelta;

printf(“%fn ”,n);

}

void main(void)

{

int j,a[]=

{15,14,12.5,10.5,5.5,6.7,9.5,11.3,9.6,10.2,10.035,9.2356,10.2356,9.3654,10.01101 };

PID

sPID;

double

rOut;

double

LastrOut=0;

/*y(k-1)*/

double

rIn;

double

Ud1 = 1;

double

Ui1 = 1;

PIDInit(&sPID);

/*PID初始化*/

sPID.K1 = 1;

sPID.T1 = 1;

sPID.T2 = 1;

sPID.T

= 1;

sPID.SetPoint = 10.0;

/*设定值*/

sPID.Err1 = 1;

for(j=0;j

{

rIn = a[j];

/*sensor();*/

/*输入*/

rOut= PIDCalc(&sPID,rIn,Ud1,Ui1);

actuator(rOut, LastrOut);

} }