www.pudn.com > mohu.rar > CHAP10_3.CPP

//chap10_3.cpp 
//Real-time PID Control 
//Options->Linker->Libraries->Graphics Library 
//Copy c:\Borlandc\bgi\*.bgi files to the current directory 
//Copy the H file "chap10_3.h" to \include directory 
//Set:(1)c:\borlandc\include;(2)c:\borlandc\lib; 
#include        //Using sin() 
#include       //Using clrscr() and getch() and kbhit() 
#include  
#include       //Using sprintf() 
#include         //Using _disable() and _enable() 
#include  
#include        //Specially for std_fun bioskey() 
#include      //Using exit() 
#include    //Interupt parameters 
 
#define OUTPORT outportb 
#define INPORT  inportb 
#define T        500 
#define DD_PORT 0x380    //D/D board base address 
 
int channel=1;            //three frame: 0:inner; 1:middle; 2:outter 
int Signal=1;             //Sine Signal 
//int Signal=2;           //Step Signal 
 
double PositionCommand[TIMER_RATIO]; 
double Line1[TIMER_RATIO]; 
double CurrentPosition[TIMER_RATIO]; 
double Line2[TIMER_RATIO]; 
 
double A,F; 
double u=0.0,ts=0.001; 
double pi=3.14159265358979; 
double timezt=0; 
int flag=0; 
unsigned short    UpdateFlag=0; 
double mStep,mStep_1,Step,SaveStep=0;  //mStep_1(t)=mStep(t-10) 
//mStep must defined double 
unsigned short    ItemNum; 
unsigned short    TimerCount; 
unsigned long     Counter; 
 
unsigned short    flagF5; 
unsigned short    FinishSimulate; 
FILE *xyz; 
 
#define DD_PORT  0x380   //Inner frame 
void ResetShuXianBiao() 
{ 
     outport(DD_PORT,0x0); 
     outport(DD_PORT,0xf); 
     outport(DD_PORT+4,0x0); 
     outport(DD_PORT+4,0xf); 
     outport(DD_PORT+8,0x0); 
     outport(DD_PORT+8,0xf); 
} 
 
double angle_1; 
double ReadD_D(unsigned short channel) 
{ 
	unsigned short Data_H1,Data_L1,Data_H2,Data_L2; 
	long int data,angle_degree,angle_minute,angle_second; 
	double angle,angle1,errory; 
	do { 
	     Data_L1=inport(DD_PORT+4*channel); 
	     Data_H1=inport(DD_PORT+2+4*channel); 
	     Data_L2=inport(DD_PORT+4*channel); 
	     Data_H2=inport(DD_PORT+2+4*channel); 
	    }while((Data_L1!=Data_L2)||(Data_H1!=Data_H2)); 
 
	 data=((Data_H1 & 0x3f)*65536+Data_L1); 
	 angle1=data*0.9;       //Second per data 
	 if((Data_H1 & 0x80) ==0x80)  //nagative if Data_H.7 ==1 
	     angle1=-angle1; 
	 angle=angle1/3600.0;   //Change from Second to degree 
	 return(angle); 
} 
 
#define Da_Board  0x1A0 
void Write_DA(double dValue, int channel) 
{ 
   unsigned int hi,low,hilow; 
   int state; 
   double voltage; 
   voltage=dValue; 
   if(voltage>2.0)voltage=2.0; 
   if(voltage<-2.0)voltage=-2.0; 
   hilow=(unsigned int)((voltage)*65535/20); 
 
   hi=hilow&0xff00; 
   hi=hi>>8; 
   low=hilow&0xff; 
 
   outportb(Da_Board+0,channel); //Select channel 
   do 
	{ 
	 state=(inportb(Da_Board+0))&0x80; 
	} while(state==1);    //read bit D7; if D7=0 write data 
 
   outportb(Da_Board+1,low);   //write low byte 
   outportb(Da_Board+2,hi);    //write high byte 
   outportb(Da_Board+3,0);     //start da 
} 
 
int KbGetKey(int *ScanCode) 
{ 
     int Key; 
     int KeyCode; 
     Key=bioskey(0); 
     if((Key&0x00ff)==0) 
     { 
	 KeyCode=0; 
	 *ScanCode=(Key>>8)&127; 
     } 
     else 
     { 
	 KeyCode=Key&0xff; 
	 *ScanCode=0; 
     } 
     return(KeyCode); 
} 
 
int SavedFlag,SaveCounter; 
void SetupKeyReaction(void) 
{ 
     int Key,Scan; 
     if(kbhit()) 
     { 
	  Key=KbGetKey(&Scan); 
	 if (Key==KB_C_N_F5 && Scan==KB_S_N_F5) 
		{ 
		 SavedFlag=1; 
		 SaveCounter=0; 
		} 
	 if(Key==KB_C_A_X && Scan==KB_S_A_X) 
	 { 
	     FinishSimulate = 1; 
	 } 
     } 
} 
 
double huge DataSaved[DATA_DIMENTION][DATA_LENGTH]; 
void DataSaveRoutine( long int SaveSpan) 
{ 
	if(SavedFlag==1) 
	  { 
	    if((SaveCounter<=SaveSpan*DATA_LENGTH)) //DATA_LENGTH is Defined by chap10_3.h 
	     { 
		int SaveIndex = SaveCounter/SaveSpan; 
//Save data(rin,yout) to xyz.dat 
		DataSaved[0][SaveIndex]=PositionCommand[ItemNum]; 
		DataSaved[1][SaveIndex]=CurrentPosition[ItemNum]; 
	     } 
	   SaveCounter++; 
	   if(SaveCounter>=SaveSpan*DATA_LENGTH) { SavedFlag=0; SaveCounter=0; } 
	  } 
	 if(SavedFlag==0)  SaveCounter=0; 
} 
 
int M; 
double yout,error,derror; 
double u_1=0,u_2=0,y_1=0,y_2=0,error_1=0,error_2=0,ei=0; 
double Control(double rin,unsigned short channel) 
{ 
M=2; 
if( M==1)    //Realtime control 
{ 
yout=ReadD_D(channel);   //Read realtime data 
CurrentPosition[ItemNum]=yout; 
} 
if( M==2 )   //Simulation control 
{ 
yout=1.94*y_1-0.94*y_2+0.0008674*u_1+0.0008503*u_2; 
} 
 
CurrentPosition[ItemNum]=yout; 
yout=CurrentPosition[ItemNum]; 
 
F=0.50; 
A=0.010; if(A==0.0) { A=0.0001; } 
rin=A*sin(F*2*pi*timezt); 
error=rin-yout; 
 
u=10.0*error+1.0*derror+0*ei; 
//Update Parameters 
y_2=y_1;y_1=yout; 
u_2=u_1;u_1=u; 
error_2=error_1; 
error_1=error; 
 
return u; 
} 
int Cycles=5;  //Display cycle times 
void DynamicDisplay() 
{ 
     //Make a window 
     char   strA[50]; 
     int    i,Spoint; 
     int    bottom,middle,top,right,left; 
     bottom=300;middle=200;top=100;left=50;right=550; 
     setcolor(RED); 
     outtextxy(270,50,"PID Controller"); 
     line(left,top,left,bottom);        //lineleft 
     sprintf(strA,"%f",A*6.0/5.0); 
     outtextxy(36,top-10,strA); 
     line(left,top,right,top);          //linetop 
     outtextxy(36,middle,"0"); 
     line(left,middle,right,middle);    //linemiddle 
     sprintf(strA,"%f",-A*6.0/5.0); 
     outtextxy(36,bottom+5,strA); 
     line(left,bottom,right,bottom);    //linedown 
     line(right,top,right,bottom);      //lineright 
 
//Make Curve Range 
     for(i=0;i