main.c

本程序是基于TI公司DSPF206芯片设计的伺服控制器的源程序。

/*

*/

#include "register.h"  
#define lcd_i  0x1801  /*E=1 RS=0 R/W=0*/
#define lcd_d  0x1805  /*E=1 RS=1 R/W=0*/
#define lcd_c  0x1800  /*E=0 RS=0 R/W=0*/

float p10,p11,p12,p13,p20;       /*System Parameters*/ 
float p00[10];                   /*定位位置适*/
long int m00,m01,m02;            /*Dispaly Parameters*/ 
unsigned int  m03;               /*Valve signal dispaly*/
int PT_counter;                   /*PT_counter:定位时间间隔计数器*/
int D_counter;                   /*D_counter:实时显示计数器10个定时器中断显示一次*/
int PM_counter;                  /*PM_counter:参数设定计数器*/  


unsigned int  P_feedback;        /*P_feedback:位移采样值*/

float         P_ins;             /*P_ins位置信号指令*/
long int      mv_ins;            /*比例阀指令*/

float bp[11]={146,306,726,1245,1665,1825,1665,1245,726,306,146};/*位移采样滤波系数 */
float x[11]={0,0,0,0,0,0,0,0,0,0,0}; 
float bv[6]={0.028393,0.1427,0.3289,0.3289,0.1427,0.028393};
float vx[6]={0,0,0,0,0,0};
float px2,px1,v2,v1,integral;


unsigned char ready[19]={"Controller is ready"};
unsigned char autom[17]={"Run automatically"};
unsigned char manu[12]={"Run manually"};
unsigned char reset[12]={"System Reset"}; 
unsigned char ld[13]={"Long-Distance"}; 
unsigned char param[10]={"Parameters"};

unsigned char dism00[10]={"M00=000.00"};  
unsigned char dism01[10]={"M01=000.00"}; 
unsigned char dism02[10]={"M02=000.00"};   




/****************************************/
/****************************************/       
/* Disable interrupts */       
 inline void  disable()
  {
	asm(" setc INTM");
  }

/* Enable interrupts */
 inline void  enable() 
  {
	asm(" clrc INTM");
  }                      
/****************************************/
/* Do uart interrupt */ 
 void interrupt uart()
  {
	return;
  }      
/****************************************/  
  /* Do timer interrupt */         
 void interrupt timer()
  { 
     adreset=0x00ff;
    asm(" clrc xf");
    asm(" setc xf");
    asm(" clrc INTM"); 
   
    if(D_counter==9)D_counter=0;
    if(PT_counter==999)PT_counter=0; 
    D_counter++;        /*实时显示计数器*/ 
    PT_counter++; 
   
 	return;
   }   

/****************************************/
  /* Do A/D int23 interrupt */         
 void interrupt adint23() 
 {  float Filter ();
    float Differential();
 
    float f_p,error,velocity,acc; /*f_p:浮点位移值*/ 
    float valve_ins;
    
    
    
            
    P_feedback=ad0;      /*位置采样*/
     
     x[10]=x[9];                        /*FIR滤波*/
	  x[9]=x[8];
	  x[8]=x[7];                                     
	  x[7]=x[6];
	  x[6]=x[5];
	  x[5]=x[4];
	  x[4]=x[3];
	  x[3]=x[2];
	  x[2]=x[1];
	  x[1]=x[0];
      x[0]=(float)P_feedback;  
	  f_p=Filter(x,bp,10)/10000;   
      f_p=f_p*210/65535;  /*转换位置值单位mm*/ 
      m01=(long int)(f_p*100);  /*反馈位置显示*/
      
      error=P_ins-f_p;    /* 偏差值*/   
      
/***********求速度*************************/ 
      px2=px1;
      px1=f_p;
       
      vx[6]=vx[5];
	  vx[5]=vx[4];
	  vx[4]=vx[3];
	  vx[3]=vx[2];
	  vx[2]=vx[1];
	  vx[1]=vx[0];
      vx[0]=Differential( px2, px1, 0.001);
      velocity=Filter(vx,bv,5);
      
      
/***********求加速度*************************/  
      v2=v1;
      v1=velocity;
      acc=Differential( v2, v1, 0.001);     
      
      
/***********求积分**************************/ 
      if((abs(error)<10)&&(abs(error)>0.1))  
      {integral=integral+error;}
      else integral=0; 
      
/***********算法*****************************/  
      valve_ins=p10*error*0.01+p11*integral*0.01+p12*velocity*0.001+p13*acc*0.00001;
      valve_ins=valve_ins*2047; 
      mv_ins=(int)valve_ins+0x7ff;
      if(mv_ins>=4095)
       { mv_ins=0xfff;} 
      if(mv_ins<=0)
       { mv_ins=0x000;} 
       
      CSDA=mv_ins; 
    return;    
 } 
/****************************************/
 
  float Filter (xx,bb,k)  /*滤波器   */
       float xx[11],bb[11];
       int k;
	  { float y;
	    int i;
		y=0;
		for(i=0;i<=k;i++)
		{
			y=xx[i]*bb[i]+y;
		}
		return(y);
	  }
  float Differential( x2, x1, t)  /*微分   */
    float x1,x2,t;
   {float dx;
    dx=(x2-x1)/t;
	return (dx);
   }  
/****************************************/

/***************************************/     
   /* Do nothing interrupt */         
 void interrupt nothing() 
 {
 	return;
 } 
/****************************************/
 void lcddelay()   /*lcd Delay*/
 { int ii;
   ii=1;
   while(ii<=100)
   { ii++;
   }
 	return;
 }              
 
 
 void   lcdcom(com)   /*lcd Instruction signal*/
  unsigned int com; 
 { IOSR=lcd_i; 
  asm(" NOP");
  CSLCD=com;      
  asm(" NOP");
  IOSR=lcd_c; 
  lcddelay();
  IOSR=0x1808; 
 	return;
 }   
             
 void  lcddat(dat)   /*lcd Data signal*/
  unsigned int dat;
 { IOSR=lcd_d;
  CSLCD=dat;
  asm(" NOP");
  IOSR=lcd_c;   
  lcddelay();    
  IOSR=0x1808; 
   	return;
 }  
 
 void  lcddispaly(com,dat)   /*lcd Dispaly */
 unsigned int com,dat;
 {	
    lcdcom(com);  
	lcddat(dat); 
 	return;
 
 }  
 
 /***************run_time Dispaly***********************/  
 void run_time_dispaly()
  { int m004,m005,m006,m008,m009;  
    int m014,m015,m016,m018,m019;
    int i,n; 
    unsigned int lcddb; 
    
  
    m00=(long int)(P_ins*100); 
    m004=m00/10000;
    m005=m00/1000-m004*10;  
    m006=m00/100-m004*100-m005*10;
    m008=m00/10-m004*1000-m005*100-m006*10;
    m009=m00-m004*10000-m005*1000-m006*100-m008*10;  
    dism00[4]=(char)(m004+0x30);
    dism00[5]=(char)(m004+0x30);
    dism00[6]=(char)(m004+0x30);
    dism00[8]=(char)(m008+0x30);
    dism00[9]=(char)(m009+0x30); 
       i=4; 	                    /*m00 Dispaly*/
	lcddb=0x98;
 	while(i<10)
    {    	 
	 lcdcom(lcddb);
	 lcddat(dism00[i]);
	 lcddb++; 	    
	 i++;	
    }
  
   
    m014=m01/10000;
    m015=m01/1000-m004*10;  
    m016=m01/100-m004*100-m005*10;
    m018=m01/10-m004*1000-m005*100-m006*10;
    m019=m01-m004*10000-m005*1000-m006*100-m008*10;  
    dism01[4]=(char)(m014+0x30);
    dism01[5]=(char)(m014+0x30);
    dism01[6]=(char)(m014+0x30);
    dism01[8]=(char)(m018+0x30);
    dism01[9]=(char)(m019+0x30); 
     i=4; 	                    /*m01 Dispaly*/
	lcddb=0xA2;
 	while(i<10)
    {    	 
	 lcdcom(lcddb);
	 lcddat(dism01[i]);
	 lcddb++; 	    
	 i++;	
    } 
    
    
    
    m03=CSDA;                         /*m03 Dispaly*/
    
    if((m03>=0x7AE)&&(m03<=0x850))
     { lcdcom(0x09);
	   lcddat(0x3E);
	   lcdcom(0x0A);
	   lcddat(0x3C);
     }
      else 
     { if(m03<0x7ff)
       n=m03/0xE3;
       i=1;
       lcddb=0x88;
 	  while(i<=n)
       {    	 
	    lcdcom(lcddb);
	    lcddat(0xFF);
	    lcddb--; 	    
	    i++;	
       } 
        
       if(m03>0x7ff)
       n=(m03-0x7FF)/0xE3;
       i=1;
       lcddb=0x8B;
 	  while(i<=n)
       {    	 
	    lcdcom(lcddb);
	    lcddat(0xFF);
	    lcddb++; 	    
	    i++;	
       } 
      }
   return;
   }   
 /****************自动运行************************/ 
  void automat()
  { int i,j;
    unsigned int lcddb;
    
    enable();	            	/* Enable all interrupts */
    CSLED=0x8000;
    i=0; 	                    
	lcddb=0xd4;
 	while(i<17)
    {    	 
	 lcdcom(lcddb);
	 lcddat(autom[i]);
	 lcddb++; 	    
	 i++;	
    }   
    
    
    i=0;                   /*定位循环*/
    while(i<2)
    { P_ins=p00[i]; 
    
 delay: IOSR=0x1800;  
        if(PT_counter<999)
        {  
          IOSR=0x1808;
          if(D_counter==9)
          { run_time_dispaly();}
          goto delay;
        }
      i++;
     
    } 
     
    IOSR=0x1800;
    
   return;
   }   
 /****************手动运行************************/
  void manual()
  { int i;
    unsigned int lcddb;
    
    enable();	            	/* Enable all interrupts */
    
    CSLED=0x1000;
    
    i=0; 	                   
	lcddb=0xd4;
 	while(i<12)
    {    	 
	 lcdcom(lcddb);
	 lcddat(manu[i]);
	 lcddb++; 	    
	 i++;	
    } 
  
  
   return;
   }   
 /****************示教************************/
  void autostudy()
  { int i;
    unsigned int lcddb;
    
    CSLED=0x2000;
    i=0; 	                  
	lcddb=0xd4;
 	while(i<12)
    {    	 
	 lcdcom(lcddb);
	 lcddat(manu[i]);
	 lcddb++; 	    
	 i++;	
    } 
  
  
   return;
   }   
 /*****************参数设定***********************/
  void paramselect()
  { int i;
    unsigned int lcddb;
    
    CSLED=0x0008;
    i=0; 	                    
	lcddb=0xd4;
 	while(i<10)
    {    	 
	 lcdcom(lcddb);
	 lcddat(param[i]);
	 lcddb++; 	    
	 i++;	
    } 
  
  
   return;
   }   
 /*****************复位***********************/
  void sysreset()
  { int i;
    unsigned int lcddb;
    
    CSLED=0x0200;
   
    enable();	            	/* Enable all interrupts */
    
    
    i=0; 	                    
	lcddb=0xd4;
 	while(i<12)
    {    	 
	 lcdcom(lcddb);
	 lcddat(reset[i]);
	 lcddb++; 	    
	 i++;	
    } 
  
   return;
   }        
 /*****************联机***********************/
  void online()
  { int i;
    unsigned int lcddb;
   
   
    CSLED=0x4000; 
    i=0; 	                    
	lcddb=0xd4;
 	while(i<19)
    {    	 
	 lcdcom(lcddb);
	 lcddat(ready[i]);
	 lcddb++; 	    
	 i++;	
    } 
  
  
   return;
   }   
 
  /*****************远程***********************/
  void l_d()
  { int i;
    unsigned int lcddb;
    
    
    CSLED=0x0040;
    i=0; 	                   
	lcddb=0xd4;
 	while(i<13)
    {    	 
	 lcdcom(lcddb);
	 lcddat(ld[i]);
	 lcddb++; 	    
	 i++;	
    } 
  
  
   return;
   }   
 /****************************************/
 /****************************************/
  
 main() 
 { 
   unsigned int  lcddb,butsample,butsample1;
   int m,i;  
   
   /*****System initialization*****/
   asm(" clrc CNF"); 
   
    disable();     /* Disable all interrupts */
    *IFR=0xFFFF;	/* Clear interrupts */
    *IMR=0x0006;	/* 中断设定 */ 
    ICR=0x0015;      
    WSGR=0x0400;    /* Set  wait states */ 
  
   /******Timer initialization,采样频率1ms****/ 
	BRD=0x4E1F;     /* 加载定时器计数寄存器 */
	TIM=0xFFFF;     /* 加载定时器周期寄存器 */
	TCR=0x0c20;     /* 加载定时器控制寄存器 */

	ASPCR=0x000f;   /* IO0-IO3 are configed as output*/
   
   /**** LCD initialization Starts****/
   IOSR=0x1800;
     m=0;           
    while(m<=500)
    { m++; 
      lcddelay(); 
    } 
    lcddb=0x0001;   /*Dispaly Clear*/ 
	lcdcom(lcddb); 
	 m=0;
    while(m<=150)   /* delay*/
    {m++; 
     lcddelay(); 
    } 	 
    	
	lcddb=0x0038;  /* Set function */  
	lcdcom(lcddb); 
    m=0;  
    while(m<=150)    /* Delay*/
    {m++; 
     lcddelay(); 
    }
	 
	lcddb=0x000c;   /*Dispaly on/off*/
	lcdcom(lcddb);  
	
	lcddb=0x0006;   /*Entry mode set*/
	lcdcom(lcddb);	  
    lcddelay();    
 /****** LCD initialization Ends*****/ 
  
 /********System Parameters initialization starts*/
    P_ins=5;
    p10=27;
    p11=0;
    p12=10;
    p13=20;
    px2=0;
    px1=0;
    v2=0;
    v1=0;
    integral=0;
    
 /********System Parameters initialization ends*/
    CSLED=0x0000;
	CSDA=0x07ff;   /* Valve mid signal*/
 /***********************************************/ 
    i=0; 	                    /*Controller is ready*/
	lcddb=0xd4;
 	while(i<19)
    {    	 
	 lcdcom(lcddb);
	 lcddat(ready[i]);
	 lcddb++; 	    
	 i++;	
    }    
    i=0; 	                    /*m00 Dispaly*/
	lcddb=0x94;
 	while(i<10)
    {    	 
	 lcdcom(lcddb);
	 lcddat(dism00[i]);
	 lcddb++; 	    
	 i++;	
    }   
    
    i=0; 	                        /*m01 Dispaly*/
	lcddb=0x9E;
 	while(i<10)
    {    	 
	 lcdcom(lcddb);
	 lcddat(dism01[i]);
	 lcddb++; 	    
	 i++;	
    }   
    lcdcom(0x89);
	lcddat(0x3E);
	lcdcom(0x8A);
	lcddat(0x3C);
    
    
 /**** Initialization Ends*********/  
 
loop: IOSR=0x1800;
      
      butsample=0x8000;
       
   
      switch(butsample)
      {case 0x8000 : automat();break;
       case 0x4000 : online();break;
       case 0x2000 : autostudy();break;
       case 0x1000 : manual();break; 
       case 0x0200 : sysreset();break;  
       case 0x0040 : l_d();break; 
       case 0x0008 : paramselect();break;
       default : IOSR=0x1808; goto loop;
      }

}