PID.rar_msp430_msp430f169_pid_pid 单片机_单片机 算法

#include <msp430x16x.h> #include "math.h" #define uchar unsigned char #define uint unsigned int void Init_clock(void) {uchar i=0; BCSCTL1=0x00; //打开XT2振荡器 DCOCTL=0x00; do {IFG1&=~ OFIFG; //清除振荡器失效标志 for(i=0Xff;i>0;i--);// 延时,等待XT2起振 } while((IFG1&OFIFG)==OFIFG);// 判断XT2是否起振 BCSCTL2=0x00; BCSCTL2=SELM_2; //MCLk,SMCLK的时钟源是高速晶振8MH,不分频 } int PID(int Ek0) { static float former_error; // 上次偏差 static float futher_error; // 上上次偏差 static float Kp = 20.-60.0; // 比例系数 static float Ki = 3-10; // 积分系数 static float Kd = 0.5-3.0; // 微分系数 static float Out; // 调整输出 static float error_1; // 偏差一阶 static float error_2; // 偏差二阶 static float Out_increament; // 输出增量 float now_error; // 当前偏差 float iout; now_error = Ek0/100; // 计算出当前偏差 error_1 = now_error-error_1; //一阶偏差=当前偏差-一阶偏差 error_2 = now_error-2*former_error+futher_error; //二阶偏差=当前偏差-2*上次偏差+上上次偏差 Out_increament = error_1+ Kd*error_2; //增量PID输出(比例和微分)=一阶+微分系数*二阶偏差 if(now_error<=2) Out_increament += Ki*now_error; //增量PID输出(积分)=增量+积分系数*当前误差 //误差在一定的范围内时候增加积分量 Out_increament *= Kp; // 计算增量PID输出(比例系数) Out += Out_increament; // 计算调整输出值（位置） 输出+=输出增量 if(Out < -10 ) Out = -10; futher_error = former_error; // 保存上次偏差 former_error = now_error; // 保存当前偏差 if(Out>0) iout = sqrt(Out); else iout = 0; if(iout > 65536 )iout = 65535; return(iout); } void main( void ) { WDTCTL=WDTPW+WDTHOLD; //停止看门狗 Init_clock(); }