main.c

PID算法程序

#include <hidef.h>      /* common defines and macros */
#include <MC9S12XS128.h>     /* derivative information */
#pragma LINK_INFO DERIVATIVE "mc9s12xs128"

int n=0;
    /////////////////////////////////////
    ////     电机   PID  定义
    /////////////////////////////////////
 
int SetPoint=0; //设定目标 Desired Value 
int  FeedBack=0;  
float KKp=4; //比例常数 Proportional Const 
float KKi=10;                   
float KKd=0; //微分常数 Derivative Const 
signed int EE0=0;  //当前误差 
signed int EE1=0; //Error[-1] 
signed int EE2=0; //Error[-2] 

signed int EError0=0,EError1=0;
signed int iiIncpid=0;

int sp=0;

int pp=0;
    /////////////////////////////////////
    ////     电机   PID
    /////////////////////////////////////
    
unsigned int Getspeed(int cch1,int cch2 ) 
{ 
   EE0=cch1-cch2; //增量计算 
   EError0=EE0-EE1;
   EError1=EE1-EE2;
   iiIncpid=(int)(KKi*EError0+KKp*EE0+KKd*(EError0-EError1));
   EE1=EE0;  
   EE2=EE1;   //存储误差，用于下次计算     
   sp+=iiIncpid;
    if(sp>2000)
    sp=2000;
   if(sp<0)
    sp=0;
   return sp;
                                          
}
void sudu(int SetPoint) 
{     

  pp=Getspeed(SetPoint,FeedBack);     
  PWMDTY45=pp;
}

//////////////////////////////      //////////////////////////////////
/*                              PPL                                 */
//////////////////////////////      //////////////////////////////////
void PLL_Init(){
  CLKSEL=0x00;      //disengage PLL to system
  PLLCTL_PLLON=1;   //turn on PLL
  REFDV=0x81;//锁相环时钟PLLCLK=2*OSCCLK*（SYNR+1）/（REFDV+1）=80MHZ
  SYNR=0x44;
  POSTDIV=0;  //  fBUS=fPLL/2=40MHZ
  while(!(CRGFLG&0x08));   //when pll is steady,then use it; 
  CLKSEL_PLLSEL=1;        //engage PLL to system
}
//////////////////////////////      //////////////////////////////////
/*                             PWM                                  */
//////////////////////////////      //////////////////////////////////       
void PWM_Init()
{ 
  PWME=0x00;
  PWMCTL=0x70; //2,3通道级联控制舵机    0,1控制电机P  3,4控制电机N
  PWMPOL=0xff;//开始输出极性为正
  PWMCAE=0x00;//左对齐输出
  PWMCLK=0x03;//Clock A B  
  PWMPRCLK=0x00;  //ClockB=40MHZ      ClockA=40MHZ
  PWMSCLA=20;     //ClockSA=ClockA/(2xPWMSCLA)=1MHZ

  PWMPER01=3333;    //   舵机频率为300HZ
  PWMDTY01=1550;
  
  PWMPER45=4000;   //     电机P 10KHz
  PWMDTY45=0;
  PWMPER23=4000;// 电机N频率5KHz
  PWMDTY23=0;// 
                //      正极性，PWMDTY/PWMPER =占空比  
  PWME=0xff; //输出使能
}
//////////////////////////////      //////////////////////////////////
//////////////////////////////      //////////////////////////////////
void PACN_Init(void)
{               
   TIOS=0X00;
   PACTL_PAEN=0;      //与TEN=1相互独立
   TSCR1=0x80;        //快速清除flag：PAOVF PAIF
   TSCR2=0x86;        //分频：
   TCNT=0xffff;
   PACNT=0;
   PACTL_PAEN=1;                      //启动脉冲累加器A 
   PACTL_PAMOD=0;             //事件计数方式 
   PACTL_PEDGE=1;      //PT7引脚上的出现上升沿时脉冲累加器计数器加1
   PPST=0X0FF;
   PERT=0X00;//电平拉高 防止干扰
} 

void  Pit0_Init(void)
{
   PITCFLMT_PITE=0;          //disable PIT
   PITCE_PCE0=1;             //enable timer channel 0
   PITMUX_PMUX0=0;           //ch0 connected to micro timer 0
   PITMTLD0=199 ;           //micro time base 0 equals 255 clock cycles
   PITLD0=1999;            //time-out period = (PITMTLD + 1) * (PITLD + 1) / fBUS.
                             //时间计算
                             //For example, for a 16 MHz bus clock, the maximum time-out period equals:
                           //20 * 20000 /40000000=10ms
   PITINTE_PINTE0=1;         //enable interupt channel 0
   PITCFLMT_PITE=1;          //enable PIT 
}

void main(void) 
{   
  DDRT=0x00;
  PLL_Init();
  PWM_Init();
  PACN_Init();
  Pit0_Init();

  
  EnableInterrupts;

  for(;;) 
  {  
   if(n<=300)
  sudu(600);
   if(n>300&&n<=600)
    sudu(900);
    if(n>600&&n<=900)
      sudu(1300);
      if(n>900&&n<=1200)
        sudu(1600);
        if(n>=1500)
         sudu(0);
  } /* wait forever */
  /* please make sure that you never leave this function */
}


//////////////////////////////      //////////////////////////////////
/**                           PIT0 SIR                              */   
//////////////////////////////      //////////////////////////////////
#pragma CODE_SEG __NEAR_SEG NON_BANKED
void interrupt 66 PIT0_ISR(void)                        
{ 	   
   PITTF_PTF0=1;    //CLER FLAG
   n++;
                                                                            
}

#pragma CODE_SEG __NEAR_SEG NON_BANKED
void interrupt 16 Speed_Catch() 
{  
   FeedBack=PACNT; 
   PACNT=0;
   TCNT=0xffff;  
   TFLG2=0X80;
}