myad.c

交交变频矢量控制DSP部分

		    {
		       w_fk=pos2-w_buf[jjj];
		       if(w_fk>700)
		         w_fk=w_fk-1024;
		       if(w_fk<-700)
		         w_fk=w_fk+1024;
		       w_fk=w_fk<<1;
		       w_buf[jjj]=pos2;
		       jjj++;
		       if(jjj==16) jjj=0; 
		    }    
		    
/********************************ia,ib,ic变换为id,iq****************************/           
		    
		    temp=ia;
            temp=temp*MySin[a_cos];
            temp2=ib;
            temp2=temp2*MySin[b_cos]; 
            temp3=ic;
            temp3=temp3*MySin[c_cos];
			temp=(temp+temp2+temp3)*2;
			id_fk=temp>>8;    /*得到id_fk,本应移14位，定标减掉6位*/  
		    
		    temp=ia;
            temp=temp*MySin[a_sin];
            temp2=ib;
            temp2=temp2*MySin[b_sin]; 
            temp3=ic;
            temp3=temp3*MySin[c_sin];
			temp=-(temp+temp2+temp3)*2;
			iq_fk=temp>>8;     /*得到iq_fk*，本应移14位，定标减掉6位*/

/********************************id,iq,if滤波**************************************/ 			
           
           if(iiii<M)             
		    {
		       id_buf[iiii]=id_fk; 
		       iq_buf[iiii]=iq_fk; 
		       if_buf[iiii]=if_fk;
		       sumId+=id_fk;
		       sumIq+=iq_fk;
		       sumIf+=if_fk;
		       iiii++; 
		    }
		    else
		    {
		       sumId=sumId+id_fk-id_buf[jjjj];
		       sumIq=sumIq+iq_fk-iq_buf[jjjj];
		       sumIf=sumIf+if_fk-if_buf[jjjj];
		       id_fk=sumId>>MM;
		       iq_fk=sumIq>>MM;
		       //if_fk=sumIf>>MM;
		       id_buf[jjjj]=id_fk; 
		       iq_buf[jjjj]=iq_fk;  
		       if_buf[jjjj]=if_fk;
		       jjjj++;
		       if(jjjj==M) jjjj=0; 
		    }
		     
		    if(bFirstTime==1)
			{
			   id_fk=0;
               iq_fk=0; 
               if_fk=0;
               bFirstTime=0;
            }                     
            
/************************励磁电流调节器********************************************/ 
        
		   	/*if按Q.7/10定标*/ 
  		    temp=if_gd-if_fk;
		    if(temp>16||temp<-16)
		    {  
		        if_out=temp*if_kp;   
		    }
		    else
		    { 
		        if_out=temp*if_kp+if_sum;
		        if_sum+=temp*if_ki;    
		        if(if_sum>20000)if_sum=20000; //限为39V 
		        if(if_sum<0)if_sum=0;
		    }
		     
		    if(if_out>20000)if_out=20000; //限为39V 
		    if(if_out<1000)if_out=1000;
		   /*Ufm=40V，按Q.9/16定标,=20480*/
		    temp=(if_out*51)>>12;      /*(uf/Ufm*256+256)*/
			n=temp+256;
			*FALPHA=MyAcos[n];       /*得到FALPHA*/  	    
		             					    
/*********************************速度调节器***************************************/
		    
		    temp=w_gd-w_fk;
		    temp2=w_fk-w_pre;   
		    if((temp2<16)&&(temp2>-16))  
 		      if((temp>16||temp<-16))
 		      {
                 w_out=temp*w_kp; 
              }
		      else
		      { 
		         w_out=temp*w_kp+w_sum; 
                 w_sum+=temp*w_ki; //电容电压是否继续积累?
		         if(w_sum>MaxI)w_sum=MaxI; //限为7A 
		         if(w_sum<-MaxI)w_sum=-MaxI; 
		      }
		      
		    if(w_out>MaxI)w_out=MaxI; 
		    if(w_out<-MaxI)w_out=-MaxI;  
		    w_pre=w_fk; 
           
/*********************************电流调节器***************************************/		    
		    /*iq,id按Q.11/16定标*/  
		    iq_gd=w_out;
		    temp=iq_gd-iq_fk;
            if(temp>256||temp<-256)
            {
		      iq_out=temp*i_kp;
		    }
		    else
		    {
		      iq_out=temp*i_kp+iq_sum;
		      iq_sum+=temp*i_ki;
		      if(iq_sum>MaxU)iq_sum=MaxU;
		      if(iq_sum<-MaxU)iq_sum=-MaxU; 
		    }
		     
		    temp=id_gd-id_fk; 
		    if(temp>256||temp<-256)
            {
              id_out=temp*i_kp;
            } 
		    else
		    { 
		      id_out=temp*i_kp+id_sum;
              id_sum+=temp*i_ki;
		      if(id_sum>MaxU)id_sum=MaxU;
		      if(id_sum<-MaxU)id_sum=-MaxU;
		    }
		      
/*********************************电压前馈**************************************/ 
           
            temp=-823;
            temp=temp*iq_gd;
            temp=temp>>9;
            temp=temp*w_gd; 
            ud_qk=temp;
            
            temp=796;
            temp=temp*w_gd;
            temp=temp*if_gd;
            temp2=285;
            temp2=temp2*iq_gd;
            uq_qk=(temp>>3)+(temp2>>1); 
            
/************************ud,uq/ua,ub,uc变换,并得到触发角**********************************/		    
		   
		    /*Um=135 V，相电压最大值，按Q.8/16定标,=34572*/
             
            temp=((ud_qk*kk)>>3)+id_out;
            temp2=((uq_qk*kk)>>3)+iq_out;
              
            if(temp>MaxU)temp=MaxU;//限为Um
		    if(temp<-MaxU)temp=-MaxU;  
		    if(temp2>MaxU)temp2=MaxU;//限为最大电压
		    if(temp2<-MaxU)temp2=-MaxU;//限为最大电压 
            
            ud=-(temp>>9); 
            uq=-(temp2>>9);  //本应缩小8倍，前面扩大64倍
		     
		    temp=ud;
            temp=temp*MySin[a_cos];
            temp2=uq;
            temp2=temp2*MySin[a_sin];
            temp=(temp-temp2)*3;
            ua=temp>>14; 
            temp=ua;
			temp=(temp*UU)>>16;    /*(ua/Um*512+512)*/
			n=temp+256;
			*AALPHA=MyAcos[n];       /*得到ua,AALPHA*/ 
			
			temp=ud;
            temp=temp*MySin[b_cos];
            temp2=uq;
            temp2=temp2*MySin[b_sin];
            temp=(temp-temp2)*3;
            ub=temp>>14;
            temp=ub;
			temp=(temp*UU)>>16;
			n=temp+256;
			*BALPHA=MyAcos[n];       /*得到ub,BALPHA*/ 
			
			temp=ud;
            temp=temp*MySin[c_cos];
            temp2=uq;
            temp2=temp2*MySin[c_sin];
            temp=(temp-temp2)*3;
            uc=temp>>14;
            temp=uc;
			temp=(temp*UU)>>16;
			n=temp+256;
			*CALPHA=MyAcos[n];       /*得到uc,CALPHA*/ 

/****************************检测通讯*********************************************/

            *R_AALPHA=0xff00;
		    *R_BALPHA=0xff00;
		    *R_CALPHA=0xff00;
		    *R_FALPHA=0xff00;
		    if((*R_AALPHA!=*AALPHA)||
		       (*R_BALPHA!=*BALPHA)||
		       (*R_CALPHA!=*CALPHA)||
		       (*R_FALPHA!=*FALPHA))
		    {
		         /*MAINSTOP=0xffff;
		         bStop=1;
		         DAC=0;*/
		         error=error+1;
		    }
		    			  
/****************************观测4个量*********************************************/		    
		    
            if(j<5000)
            {
              j++;    
            }
            else
            {
              if(i<SIZE)
              {    
                buffer1[i]=theta;
                buffer2[i]=w_gd; 
                buffer3[i]=w_fk;
                
                buffer4[i]=iq_gd;
                buffer5[i]=iq_fk;
                buffer6[i]=iq_out>>9;
                
                buffer7[i]=id_gd;
                buffer8[i]=id_fk;
                buffer9[i]=id_out>>9;
                
                buffer10[i]=ud_qk>>9; 
                buffer11[i]=uq_qk>>9;
                buffer12[i]=-ud;
                buffer13[i]=-uq;
                
                buffer14[i]=ua;
                buffer15[i]=ub;
                buffer16[i]=uc;
                
                buffer17[i]=ia;
                buffer18[i]=ib;
                buffer19[i]=ic;
                
                buffer20[i]=if_gd;
                buffer21[i]=if_fk;
                
                buffer22[i]=if_out; 
                
                i++;            
              }
              if(i==SIZE)
              { 
                 /*MAINSTOP=0xffff;
		         /*bStop=1;*/
                 i=SIZE;
              } 
            }
             
/***************************观测四路AD********************************************/		    
	        temp=ia<<2;
		    temp=temp+2048;
		    DA0=temp;
		    
            temp=ia<<2;
		    temp=temp+2048;
		    DA1=temp;
		    
		    temp=ia<<2;
		    temp=temp+2048;
		    DA2=temp;
		    
            temp=ia<<2;
		    temp=temp+2048;
		    DA3=temp;
		
		    DA4=0; 
/********************************************************************************/	 
           
            *ADCTRL2=*ADCTRL2|0X4200;	// 复位SEQ1，且清除INT FLAG SEQ1标志写"1"清0 
            asm(" NOP");
            asm(" NOP");
            asm(" NOP");
            asm(" NOP"); 
            t=15-t;
            DAC=t;
            asm(" clrc INTM");
}   

main()
{
   asm(" setc INTM");	
   initial( );
   ValueInit( );
   ADINIT( );
   asm(" clrc INTM");
   ADSOC( );  
   while(!(bStop))
   {}
}