pid.c

利用DSP实现PID控制功能

#include "DSP28_Device.h"
#include "DSP28_Adc.h"
#include "DSP28_PID.h"
#include "DSP28_Ram.h"
#include <stdio.h>
#include <stdlib.h>
interrupt void adc_isr(void);//ADC中断地址

//---------------------------------------------------------------------------
//struct _pid warm,*pid;
//float process_point,set_point,dead_band;
//float p_gain,i_gain,d_gain,integral_val,new_integ;
//float integral_val;
/*---------------------------------------------------------------------------
pid_init
  DESCRIPTION This function initializes the pointers in the _pid sturcture to
  the process variable and the setpoint.
---------------------------------------------------------------------------*/
void pid_init(struct _pid *warm, float process_point, float set_point)
 {
  struct _pid *pid;
  pid = warm;
  pid->pv = process_point;
  pid->sp = set_point;
 }
/*---------------------------------------------------------------------------
pid_tune
  DESCRIPTION Sets the proportional gain(p_gain),integral gain(i_gain),
  derivitive gain(d_gain),and the dead band(dead_band) of a pid control
  structure _pid.
---------------------------------------------------------------------------*/
void pid_tune(struct _pid *pid, float p_gain, float i_gain, float d_gain, 
float dead_band,float integral_val)
 {
  pid->pgain = p_gain;
  pid->igain = i_gain;
  pid->dgain = d_gain;
  pid->deadband = dead_band;
  pid->integral = integral_val;
  pid->last_error = 0;
  
 }
/*---------------------------------------------------------------------------
pid_setinteg
  DESCRIPTION Set a new value for the integral term of the pid equation.
  This is useful for setting the initial output of the pid controller at 
  start up
---------------------------------------------------------------------------*/
void pid_setinteg(struct _pid *pid,float new_integ)
 {
  pid->integral = new_integ;
  pid->last_error = 0;
 }
/*---------------------------------------------------------------------------
pid_bumpless
  DESCRIPTION Bumpless transfer algorithim.When suddenly changing setpoints,or
  when restarting the PID equation after an extended pause,the derivative of 
  the equation can cause a bump in the controller output.This function will 
  help smooth out that bump.The process value in *pv should be the updated 
  just before this function is used. 
---------------------------------------------------------------------------*/
void pid_bumpless(struct _pid *pid)
 {
  pid->last_error = (pid->sp)-(pid->pv);
 }
/*---------------------------------------------------------------------------
pid_calc
  DESCRIPTION Performs PID calculations for the _pid structure *a.This function
  uses the positional form of the pid equation,and incorprates an integral 
  windup prevention algorithim.Rectangular integration is used,so this function
  must be repeated on a consistent time basis for accurate control.需要修改
---------------------------------------------------------------------------*/
/*float pid_calc(struct _pid *pid)
{
 float err,err_t;
 float pterm,dterm,result,ferror;
 err_t = (pid->sp)-(pid->pv);
 if(err_t>=0)
  err=err_t;
 else
  err=-err_t;
 if(err>pid->deadband)
 {
  ferror = (float) err;
  pterm = pid->pgain*ferror;
  if(pterm>100||pterm<-100)
   {
    pid->integral = 0.0;
   }
   else
    {
     pid->integral += pid->igain*ferror;
	 if(pid->integral > 100.0)
	  {
	   pid->integral = 100.0;
	  }
	  else if (pid->integral < 0.0) 
	   {
	    pid->integral = 0.0;
	   }
    }
  dterm = ((float)(err-pid->last_error)) * pid->dgain;
  result = pterm + pid->integral + dterm;
 }
  else 
  result = pid->integral;
  pid->last_error=err;
  return (result);
}  */

//全局变量定义：
//AD采样变量
Uint16 LoopCount = 0;
Uint16 Count = 0;
Uint16 i,j;
float Voltage1[10] = {0,0,0,0,0,0,0,0};
float Voltage2[10] = {0,0,0,0,0,0,0,0};
float vout = 0,v;
//PID控制变量
Uint16 flag = 0;
Uint16 f=0;
float adjust_value;
float allow_error = 0.0001;
float actual_error = 0;
float error;
float err;
float pterm=0;
float pterm_t=0;
float iterm=0;
float dterm=0;
float uterm=0;
float uterm_t=0;
float deltu=1;
//float err;
//float pterm,dterm,result,ferror;

struct _pid warm,*pid;
float process_point;
float set_disp =5.2;//1.76,4.21,4.249,4.72
float p_gain = 1.73; //5.2
float i_gain = 0.026;  //0.056
float d_gain = 0.8289;//0.18   ,  0.0089
float dead_band = 0.01;
float integral_val = 0.01;

float actual_disp = 0;
float max_disp = 9.8; 
//pid=&warm;
//Ram数据变量

float up=0,down=0,u0,u=0;
unsigned int t;
struct data a[120];
//向FPGA写数据函数
unsigned int  *p=(unsigned int *) 0x2000;
void Write_to_FPGAreg1(void)
{
 unsigned int i,j;
// unsigned int  *p=(unsigned int *) 0x2000;
 *p=0x3F0;
 *(p+1)=0x0000; 
 *(p+2)=0x0000;
 *(p+3)=0x00;
 *(p+4)=0x00;
 *(p+5)=0x00;
 *(p+6)=0x00;
 //for(k=0;k<=10;k++)

 for(i=0;i<=1;i++)
  {
   for(j=0;j<=600;j++);
  }
  *p=0x3F0;
  *(p+1) = 0;//4095/150 * up;
  *(p+2) = 0;//4095/150 * up;
 //for(i=0;i<=1000;i++)
 // {
 //  for(j=0;j<=60000;j++);
 // }
 // *p=0x3F0;
  //*(p+1) = 4095/150 * (up-down);
  //*(p+2) = 4095/150 * (up-down);
 }


/*************************PID算法**************************/ 

void main (void)
{
 
//初始化系统控制寄存器 、PLL、看门狗和时钟
 InitSysCtrl();

//时钟初始化HSPCLK=SYSCLKOUT/6=75MHz
 EALLOW;
 SysCtrlRegs.HISPCP.all=0x0;
 EDIS;

//GPIO设置
 InitGpio();

//初始化PIE相量表
//禁止和清除所有CPU中断
 DINT;
 IER=0x0000;
 IFR=0x0000;
 InitPieCtrl();
 InitPieVectTable();

//初始化外设模块
 InitAdc();

//重新分配中断向量
 EALLOW;
 PieVectTable.ADCINT=&adc_isr;
 EDIS;

//使能PIE中的ADCINT中断
 PieCtrl.PIEIER1.bit.INTx6=1;

//使能CPU中断
 IER|=M_INT1;    //使能全局中断INT1

 //使能全局中断和高优先级适时调试功能
 EINT;       //使能全局中断INTM
 ERTM;         //使能全局适时调试中断DBGM
 pid=&warm;

  pid_init(&warm,process_point,set_disp);
  pid_tune(&warm,p_gain,i_gain,d_gain,dead_band,integral_val);
  pid_setinteg(&warm,1.0);
  pid->last_error = 0;
 // for( t=0;t<120;t++)
  //{
  // if (set_disp == a[t].num)
  //   {
      
   //   up = a[t].x;
  //    down = a[t].y;
	  Write_to_FPGAreg1(); 
	 // for(i=0;i<=200;i++)
       // {
        // for(j=0;j<=60000;j++);
       // }
     // t = 120;
    //  u0 = up - down;
  	// }
  //}
   


 AdcRegs.MAX_CONV.all=0x0001;//设置SEQ1的2转换
 AdcRegs.CHSELSEQ1.bit.CONV00=0x08;//设置ADCINB0作为SEQ1的第一个转换
 AdcRegs.CHSELSEQ1.bit.CONV01=0x0A;//设置ADCINB1作为SEQ1的第二个转换
 AdcRegs.ADCTRL2.bit.EVA_SOC_SEQ1=1;//使能EVASOC启动SEQ1
 AdcRegs.ADCTRL2.bit.INT_ENA_SEQ1=1;//使能SEQ1中断

//配置时间管理器EVA
 EvaRegs.GPTCONA.bit.T1STAT=1;
 EvaRegs.T1CNT=0x0000;
//EvaRegs.T1CMPR=0x0080;//设置T1比较值
// EvaRegs.T1CMPR=0x0080;
 EvaRegs.T1PR=0x2FFF;//设置周期寄存器
 EvaRegs.GPTCONA.bit.T1TOADC=2;//通用定时器启动ADC，使能事件管理器A的EVASOC
 EvaRegs.T1CON.all=0x1042;//使能定时器1比较（递增计数模式）
//bit.T1TOADC
//等待ADC中断
 while (1)
   {
      LoopCount++;
    //  if(LoopCount==1)
      //  for(;;);	 
   }
 


//for(;;);

}


//AD采样中断服务程序
interrupt void adc_isr(void)
{
  Voltage1[Count] = AdcRegs.RESULT0 * 3.0/65520;
  Voltage2[Count] = AdcRegs.RESULT1 * 3.0/65520;
  
  vout += AdcRegs.RESULT0 * 3.0/65520;
  
  if(Count==9)
  {
   
    Count = 0;
    v = vout/10;
    actual_disp = max_disp * v/3.0; 
  //  pid_bumpless(&warm);
    actual_error = set_disp-actual_disp;
    vout = 0;
    
	 if(actual_error>=0)
       error = actual_error;
	 else
	   error = -actual_error;
       if (error > 0)
        {
		 err = actual_error;
		 pterm_t = pid->pgain * actual_error;
		 if (pterm_t>10)
		   {
		    pterm = 10;
		   }
		 else if (pterm_t<-10)
		   {
		    pterm = -10;
		   }
		 else
		   {
		    pterm = pterm_t;
		   }
		 iterm += pid->igain * err;
		 dterm = ((float)(err-pid->last_error)) * pid->dgain;
		 pid->last_error = actual_error;
		 uterm_t = pterm + iterm + dterm;
         uterm = uterm_t * 150.0/max_disp;
		 u = uterm;
		 if(u>140)
		  {
		   u =20;// set_disp*15;
		    //*(p+1) = u * 4095/150;
		   //*(p+2) = u * 4095/150;
		  }
		 else if (u<0)
		  {
		   u = 0;//u0-deltu;
		   *(p+1) = u * 4095/150;
		   *(p+2) = u * 4095/150;
		   

		  }
		 else
		  {
           u = u;
		   //for(i=0;i<=2;i++)
             //{
               //for(j=0;j<=60000;j++);
            // }
		   *(p+1) = u * 4095/150;
		   *(p+2) = u * 4095/150;
		   f+=1;
		  // if(f==1)
		   //for(;;);

		   }
		  deltu=deltu+1;
		 //if(deltu==2)
         // for(;;); 
		}
	  // else
         
        // for(;;);
  }
 else Count++;

 //重新初始化下一个ADC排序
 AdcRegs.ADCTRL2.bit.RST_SEQ1=1;  //复位SEQ1
 AdcRegs.ADC_ST_FLAG.bit.INT_SEQ1_CLR=1; //清除INT SEQ1
 PieCtrl.PIEACK.bit.ACK1=1;   //响应中断//

 //for(Count=0;Count<10;Count++)
 //{vout+=V[Count];
 // vout=vout/10;}
 process_point=v;
  

 return;
}