main.c

MSP430F169单片机的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();
}