pid.c

基于DSPLF2407A的PID算法

#include "2407c.h"                    
#include "PID.h"
#include "lcd.h"
ioport unsigned int port8000;
ioport unsigned int port8003;	
ioport unsigned int port8004;	
ioport unsigned int port8006;	
ioport unsigned int port8005;


// 大液晶控制板控制字  ----------------------------------------------------------
#define CTRSTATUS port8000 //port8000
#define CTRLED port8004 // port8004
#define MCTRKEY port8005  // port8005
#define CTRCLKEY port8006 // port8006
#define CTRSTATUS port8000 //port8000
#define CTRGR port8000 //port8000
#define CTRMOTORBSPEED port8003

void LCDPutString(unsigned int *pData,int x,int y,unsigned int nCharNumber,unsigned color);
void PIDControl(int rk,int yk);
void Delay(unsigned int nTime);		// 延时子程序
void PrintParameters();
void interrupt gptime1(void);     
void gp_init(void);   

int i=0,nnn;
unsigned int nCount,nCount_1=1,nCount1,nCount2,nCount3,nCount4,nJSSpeed,nJSSpeed_1=1,uWork;
unsigned int uLBD;
float a=0.6f,b=0.2f,c=0.1f,duk;
int ek,ek1,ek2,tz;
int nInput;
int nSSS,pwm,pwm1;
int md,wc;
unsigned int nScreenBuffer[30*128],pidnCount3,piduN;
int m_nSpeed,m_bPCSet;
unsigned int ccc;
int uN;
main()
{   
    unsigned int uWork1;
	int inputspeed;
	int breakflage;
	int speed[100],sp,lj;
	float ljh;
	int i,w1,w2,w3;
	
	*WSGR=0x09;
	*WDCR=0x6f;
	*WDKEY=0x5555;
	*WDKEY=0xaaaa;    
	*SCSR1=0x81fe;    //40m      
	
	asm(" setc INTM");
	for ( sp=0;sp<100;sp++ )	speed[sp]=0;
	for ( sp=0;sp<(30*128);sp++ )	nScreenBuffer[sp]=0;
	sp=nSSS=nCount=nCount1=nCount2=nCount3=nCount4=nJSSpeed=0; nInput=tz=wc=0;
	ek=ek1=ek2=0;
	uN=30; md=70; pwm1=60;
	m_nSpeed=70; m_bPCSet=0;
    inputspeed=0;
    uWork1=0;
    breakflage=0;
	
    CTRGR=0x80;
	CTRGR=0;
	CTRGR=0x80;
   	
    
	uWork=(*MCRC);
	uWork&=0x0ffc0;	/* PWM11/IOPE5,PWM12/IOPE6 for normal I/O ports */
	(*MCRC)=uWork;
	gp_init();   
	*IMR=0x3;
	*IFR=0xffff;    
	nCount=0;
	uWork=(*WSGR);
	uWork&=0x0fe39;
	(*WSGR)=uWork;
	
	// 设置显示参数和内容
	LCDSetDelay(1);		// 设置延时等待参数
	LCDSetScreenBuffer(nScreenBuffer);	// 显示缓冲区
	LCDTurnOn();			// 打开显示
	LCDCLS();				// 清除显示内存   
	LCDPutCString(str1,0,127,8,0);
	LCDPutCString(str2,0,111,2,1);
	LCDPutCString(str3,68,111,2,1);
	LCDPutCString(str6,0,95,2,1);
	LCDPutCString(str5,68,95,2,1);
	LCDPutCString(str7,0,79,3,1);
	LCDPutCString(str4,68,79,2,1);
	ShowParameters();		// 参数显示
    CTRGR=01;   
	asm(" clrc INTM");         
	Delay(128);


	for(;;)
	{

	//读键盘值
	 uWork1=MCTRKEY;
	 uWork1 &=0xff;
	 CTRCLKEY=0;
	 if(uWork1==4)
	 {		Delay(500);
		    uWork1=MCTRKEY;
	        uWork1 &=0xff;
	        CTRCLKEY=0; 
	        
		    md+=10;
		    ShowParameters();	// 显示各参数值到LCD
	 }
	 if(uWork1==8)
	 {		Delay(500);
		    uWork1=MCTRKEY;
	        uWork1 &=0xff;
	        CTRCLKEY=0; 
	        
		    md-=10;
		    ShowParameters();	// 显示各参数值到LCD
	 }
	 if(uWork1==1)
	 {		
		    Delay(500);
		    uWork1=MCTRKEY;
	        uWork1 &=0xff;
	        CTRCLKEY=0;
		    md+=1;
		    ShowParameters();	// 显示各参数值到LCD
	 } 
	 if(uWork1==2)
	 {		
		    Delay(500);
		    uWork1=MCTRKEY;
	        uWork1 &=0xff;
	        CTRCLKEY=0;
		    md-=1;
		    ShowParameters();	// 显示各参数值到LCD
	 }   
	  Delay(5);//延时，
		if (nJSSpeed_1==0)	// 读取速度标志
		{ 
		    nJSSpeed_1=1; 
			nJSSpeed=0;
			//ccc=CTRMOTORBSPEED;	// 从端口读取速度计数
			ccc=port8003;
			ccc=ccc&0xff;
			nSSS=ccc; 
			if ( nSSS>=0 && nSSS<400 )	// 合法性检测
			{
				speed[sp]=nSSS; 		// 读取66个计数值
				sp++; sp%=16;
			}
			if ( sp==0 )	// 是否已经读了66个速度？
			{				// 以下求速度平均值
				lj=0; ljh=0;
				for ( i=0;i<16;i++ )
				{
					if ( speed[i]>=0 && speed[i]<400 )
					{
						ljh+=speed[i];
						lj++;
					}
				}
				wc=( lj==0 )?(0):(ljh/lj);
				if ( wc>300 )
				{	
					wc=0;
				}
				//nCount3++; nCount3%=3;
				//if ( nCount3==2 )		
				//{
					PIDControl(md,wc);	// 调用PID算法控制程序进行控制
					uN=(100-pwm1)/2;			// 利用占空比调整控制
					ShowParameters();	// 显示各参数值到LCD
			//	}
				//PrintParameters();
				nJSSpeed_1=1;
			}
		}
    }
}               
#define Hz200 50
#define Hz1 10000
void interrupt gptime1(void)
{  int fuzhi=0;
   uWork=(*PIVR);
   switch(uWork)
   {  
   		case 0x27:
   		{
			(*EVAIFRA)=0x80;
			//key();
			uWork=(*PEDATDIR);
			uWork|=0x6000;//设置pwm11和pwm12为io输出模式
	        if(nCount1<=uN) 
	        {uWork=(*PEDATDIR);
	        uWork&=0x60DF;
	        (*PEDATDIR)=uWork;}
	        if(nCount1>uN) 
	        {uWork=(*PEDATDIR);
	        uWork|=0x6020;
	        (*PEDATDIR)=uWork;}
	         nCount1++; nCount1%=Hz200;
            if ( nCount2==0 )
            {
	        	
	        	if(nnn==0) {uWork=(*PEDATDIR);
			uWork|=0x6000;uWork&=0x60BF;(*PEDATDIR)=uWork;}
	        	if(nnn==1) {uWork=(*PEDATDIR);
			uWork|=0x6000;uWork|=0x6040;(*PEDATDIR)=uWork;}
	        	nnn=1-nnn;
            }
  			nCount2++; nCount2%=Hz1;
	        nJSSpeed++; nJSSpeed%=12000/8;	// 读取速度标志
	        if(nJSSpeed==0) 
	        {nJSSpeed_1=0;}
	        /* nCount++; nCount%=51200;
	        if(nCount==0) 
	        {nCount_1=0;}*/
   		   	break;
   		}
   }
}                       

void gp_init(void)
{
   *EVAIMRA = 0x80;
   *EVAIFRA = 0xffff;
   *GPTCONA = 0x0100;
   //*T1PR    = 0xe80;//烧到flash中运行用此参数
   *T1PR    = 0x200; //ram中运行用此参数
   *T1CNT   = 0;
   *T1CON   = 0x1040;
}



void Delay(unsigned int nDelay)
{
	int i,j,k;
	
	for ( i=0;i<nDelay;i++ )
		for ( j=0;j<16;j++ )
			k++;
}

// PID算法控制子程序-------------------------------------------------------------------------
void PIDControl(int rk,int yk)
{
	ek=rk-yk;
	duk=a*ek+b*ek1+c*ek2;		// 计算控制输出
	ek2=ek1; ek1=ek;
	if ( duk>5 )	duk=4;		// 幅度限制
	if ( duk<-4 )	duk=-4;		// 幅度限制
	tz=(int)duk;
	pwm1+=tz;					// 计算当前占空比
	if ( pwm1<0 )	pwm1=0;
	else if ( pwm1>99 )	pwm1=99;
}
void LCDPutString(unsigned int *pData,int x,int y,unsigned int nCharNumber,unsigned color)
{
	int i,j,l;
	unsigned int k,mcolor;
	
	for ( l=0;l<nCharNumber;l++ )
		for ( i=0;i<8;i++ )
		{
			k=1;
			for ( j=0;j<16;j++,k<<=1 )
			{
				if ( color==2 )	mcolor=2;
				else
				{
					mcolor=( pData[l*8+i]&k )?(1):(0);
					if ( color==0 )	mcolor=1-mcolor;
				}
				LCDPutPixel(x+l*8+i,y-j,mcolor);
			}
		}
}

// 显示参数到LCD
void ShowParameters()
{
	int w1,w2,w3;
	
	w1=md%1000/100; w2=md%100/10; w3=md%10;
	LCDPutString(numbers+w1*8,36,111,1,1);
	LCDPutString(numbers+w2*8,44,111,1,1);
	LCDPutString(numbers+w3*8,52,111,1,1);
	w1=wc%1000/100; w2=wc%100/10; w3=wc%10;
	LCDPutString(numbers+w1*8,104,111,1,1);
	LCDPutString(numbers+w2*8,112,111,1,1);
	LCDPutString(numbers+w3*8,120,111,1,1);
	if ( ek>=0 )
	{
		LCDPutString(numbers+88,36,95,1,1);
		w3=((int)ek)%100;
	}
	else
	{
		LCDPutString(numbers+96,36,95,1,1);
		w3=((int)(-ek))%100;
	}
	w1=w3%100/10; w2=w3%10;
	LCDPutString(numbers+w1*8,44,95,1,1);
	LCDPutString(numbers+w2*8,52,95,1,1);
	if ( tz>=0 )
	{
		LCDPutString(numbers+88,104,95,1,1);
		w3=tz;
	}
	else
	{
		LCDPutString(numbers+96,104,95,1,1);
		w3=(-tz);
	}
	w1=w3%100/10; w2=w3%10;
	LCDPutString(numbers+w1*8,112,95,1,1);
	LCDPutString(numbers+w2*8,120,95,1,1);
	w1=pwm1%100/10; w2=pwm1%10;
	LCDPutString(numbers+w1*8,44,79,1,1);
	LCDPutString(numbers+w2*8,52,79,1,1);
	LCDPutString(numbers+80,60,79,1,1);
	LCDRefreshScreen();
}

int wwcc;
void PrintParameters()
{
	wwcc=wc-md;
	printf("测速[%3d] 设置[%3d] 误差[%+4d] PID调整量[%+3d] 占空比[%3d%%]\n",
			wc,md,wwcc,tz,pwm1);
}

// No more.
//========================================