PID.rar_pid_pid 学习

//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 <math.h> //Using sin() #include <conio.h> //Using clrscr() and getch() and kbhit() #include <graphics.h> #include <stdio.h> //Using sprintf() #include <dos.h> //Using _disable() and _enable() #include <string.h> #include <bios.h> //Specially for std_fun bioskey() #include <stdlib.h> //Using exit() #include <chap10_3.h> //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<TIMER_RATIO;i++) //Plot 10 points once a time { Spoint=mStep_1/Step-TIMER_RATIO+i; //Start point Spoint=Spoint/Cycles; //25000/(10*T)=One Cycle if(Spoint%T==0) { clrscr(); } putpixel(Spoint-(Spoint/T)*T+50,-100*5.0/6.0* (Line1[i]/A)+200,BLUE); //Practical output putpixel(Spoint-(Spoint/T)*T+50,-100*5.0/6.0* (Line2[i]/A)+200,RED); //Ideal output putpixel(Spoint-(Spoint/T)*T+50,-100*5.0/6.0* u+200,BLACK); //Control output } } void IniTimer( unsigned char timer_mode ) //Initial 8253 { unsigned char value; _disable(); //Seting control word OUTPORT(TIMER_BASE+3,timer_mode); value = TIMER_VALUE ; //Low 8 bits OUTPORT( TIMER_BASE , value ); value = TIMER_VALUE / 256 ; //High 8 bits OUTPORT( TIMER_BASE , value ); _enable(); } void interrupt ( *OldIrqVect )( __CPPARGS ); void interrupt TimerIrqVect( __CPPARGS ) //Interupt processing { unsigned short i; _disable(); ItemNum = Counter%TIMER_RATIO; if(Counter%20000==0) { Counter=0; SaveStep=SaveStep+mStep; mStep=0; } mStep += Step; //k*ts: TIMER_CYCLE / TIMER_RATIO; if (Signal==1) //Dynamic Signal { PositionCommand[ItemNum]=A*sin(F*2*pi*mStep); } if (Signal==2) //Static Signal { PositionCommand[ItemNum]=A; } u=Control(PositionCommand[ItemNum],channel); Write_DA(u,channel); DataSaveRoutine(1); Counter++; if((--TimerCount)==0) //Updating 1 times while interupted by 10 times { UpdateFlag=1; mStep_1=mStep; TimerCount=TIMER_RATIO; } OUTPORT(EOI,0x20); //OCW2 value:0010 0000 _enable(); } void IrqHook( unsigned short irqnumber, void interrupt ( *newVect )( __CPPARGS ) ) { _disable(); OldIrqVect = getvect ( irqnumber ); setvect ( irqnumber , newVect ); _enable(); } void ReleaseHardware() { _disable(); setvect (Irqnumber,OldIrqVect); Write_DA(0.0,0); Write_DA(0.0,1); Write_DA(0.0,2); _enable(); } main(void) { int i,j,k; unsigned char timer0_mode,strtime[10]; int driver,mode; driver=DETECT; initgraph(&driver,&mode,""); timer0_mode=T8253_MODE_3|T8253_CHANNEL_0| T8253_BIN_MODE|T8253_LOW_FIRST; //8253 timer setting Step=TIMER_CYCLE/TIMER_RATIO; //Step=0.01/10=0.001s=ts mStep=0; mStep_1=0; Counter=0; TimerCoun