svmvf.c

PMSM的速度检测程序 基于2407的 参考别人的

     cmp1=0;         
     cmp2=(unsigned int)(ta+tb+tc/2);         
     cmp3=(unsigned int)(tb+tc/2);    
     cmp4=(unsigned int)(ts2);         
     cmp5=0;                 
     cmp6=(unsigned int)(tc)>>1;
     }  
   }
 }
/******************************3 sector*****************************/
else if ((theta>=2*pi3)&&(theta<pi)) 
{ 
    index=(unsigned int)(theta*40.744);
    if (volts*sin_tabn[index]<=vdcbeta)
    {
      triangle=1; 
      ta=kp*ts*sin_tab[index];
      tb=(ts2)*(1-2*kp*sin_tabn[index]);
      tc=(-kp)*ts*sin_tabp[index]; 
      if (theta<5*pi6)
      {
        cmp1=0;                
        cmp2=(unsigned int)(ta/2+tb);      
        cmp3=(unsigned int)(ta)>>1;          
        cmp4=(unsigned int)(ts2);        
        cmp5=0;               
        cmp6=(unsigned int)(ta/2+tb+tc);  
       } 
      else
       {
        cmp1=0;                
        cmp2=(unsigned int)(ta+tb+tc/2);      
        cmp3=(unsigned int)(ta+tc/2);          
        cmp4=(unsigned int)(ts2);        
        cmp5=(unsigned int)(tc)>>1;               
        cmp6=(unsigned int)(ts2);
        }
      } 
      else if(volts*sin_tab[index]>vdcbeta)
       {
      triangle=2;
      ta=ts*(1-kp*sin_tabn[index]);
      tb=(-kp)*ts*sin_tabp[index];
      tc=(ts2)*(2*kp*sin_tab[index]-1);
      cmp1=0;                
      cmp2=(unsigned int)(ta)>>1;         
      cmp3=(unsigned int)(ta/2+tb+tc);   
      cmp4=(unsigned int)(ts2);         
      cmp5=0;                
      cmp6=(unsigned int)(ta/2+tb);      
     }
   else if((-volts)*sin_tabp[index]>vdcbeta)
     {
       triangle=4;
       ta=(-ts2)*(1+2*kp*sin_tabp[index]);
       tb=kp*ts*sin_tab[index];
       tc=ts*(1-kp*sin_tabn[index]);
       cmp1=0;                
       cmp2=(unsigned int)(tc)>>1;        
       cmp3=(unsigned int)(tc/2+ta+tb);    
       cmp4=(unsigned int)(ts2);         
       cmp5=(unsigned int)(tc/2+ta);          
       cmp6=(unsigned int)(ts2);
       }        
    else 
    {
       triangle=3;
       ta=(ts2)*(1+2*kp*sin_tabp[index]);
       tb=(ts2)*(2*kp*sin_tabn[index]-1);
       tc=(ts2)*(1-2*kp*sin_tab[index]);
        if(theta<5*pi6) 
        {
        cmp1=0;                 
        cmp2=(unsigned int)(ta)>>1;          
        cmp3=(unsigned int)(ta/2+tb);       
        cmp4=(unsigned int)(ts2);        
        cmp5=0;                
        cmp6=(unsigned int)(ta/2+tb+tc);    
        } 
       else
        {
        cmp1=0;                 
        cmp2=(unsigned int)(ta+tc/2);          
        cmp3=(unsigned int)(ta+tc/2+tb);       
        cmp4=(unsigned int)(ts2);        
        cmp5=(unsigned int)(tc)>>1;                 
        cmp6=(unsigned int)(ts2);  
        } 
    }   
}
/********************************************/
/*4 sector*/
/********************************************/
else if((theta>=pi)&&(theta<4*pi3)) 
{  
   index=(unsigned int)(theta*40.744);
   if ((-volts)*sin_tabp[index]<=vdcbeta)
     {
      triangle=1;
      ta=kp*ts*sin_tabn[index];
      tb=(ts2)*(1+2*kp*sin_tabp[index]);
      tc=(-kp)*ts*sin_tab[index]; 
      if (theta<=7*pi6) 
      {
       cmp1=0;                   
       cmp2=(unsigned int)(ta/2+tb+tc);       
       cmp3=(unsigned int)(ta)>>1;             
       cmp4=(unsigned int)(ts2);             
       cmp5=(unsigned int)(ta/2+tc);           
       cmp6=(unsigned int)(ts2);             
      } 
      else
      { 
       cmp1=0;                   
       cmp2=(unsigned int)(tb+tc/2);       
       cmp3=0;             
       cmp4=(unsigned int)(ta+tb+tc/2);             
       cmp5=(unsigned int)(tc)>>1;           
       cmp6=(unsigned int)(ts2);  
       }
     } 
      else if(volts*sin_tabn[index]>vdcbeta) 
      {
      triangle=2;
      ta=ts*(1+kp*sin_tabp[index]);
      tb=(-kp)*ts*sin_tab[index];
      tc=(ts2)*(2*kp*sin_tabn[index]-1);
      cmp1=0;                 
      cmp2=(unsigned int)(ta)>>1;          
      cmp3=(unsigned int)(ta/2+tc);       
      cmp4=(unsigned int)(ts2);          
      cmp5=(unsigned int)(ta/2+tb+tc);         
      cmp6=(unsigned int)(ts2);        
      }
     else if((-volts)*sin_tab[index]>vdcbeta)
       {
       triangle=4;
       ta=(-ts2)*(1+2*kp*sin_tab[index]);
       tb=kp*ts*sin_tabn[index];
       tc=ts*(1+kp*sin_tabp[index]);
       cmp1=0;                
       cmp2=(unsigned int)(tc)>>1;         
       cmp3=0;                
       cmp4=(unsigned int)(tc/2+tb);       
       cmp5=(unsigned int)(tc/2+ta+tb);       
       cmp6=(unsigned int)(ts2);        
        }
      else 
      {
       triangle=3;
       ta=(ts2)*(1+2*kp*sin_tab[index]);
       tb=(-ts2)*(1+2*kp*sin_tabp[index]);
       tc=(ts2)*(1-2*kp*sin_tabn[index]);
       if (theta<7*pi6)
       {
        cmp1=0;                 
        cmp2=(unsigned int)(ta/2+tc);        
        cmp3=(unsigned int)(ta)>>1;           
        cmp4=(unsigned int)(ts2);          
        cmp5=(unsigned int)(ta/2+tb+tc);      
        cmp6=(unsigned int)(ts2);         
       } 
       else
       { 
        cmp1=0;                 
        cmp2=(unsigned int)(tc)>>1;        
        cmp3=0;           
        cmp4=(unsigned int)(ta+tb+tc/2);          
        cmp5=(unsigned int)(tb+tc/2);      
        cmp6=(unsigned int)(ts2);       
       }
    }                         
}
/**************************************************************************/
/***********************************sector5********************************/
/**************************************************************************/
else if((theta>=4*pi3)&&(theta<5*pi3)) 
{      
         index=(unsigned int)(theta*40.744);
         if ((-volts)*sin_tab[index]<=vdcbeta)
        {
          triangle=1;
          ta=(-kp)*ts*sin_tabp[index];
          tb=(ts2)*(1+2*kp*sin_tab[index]);
          tc=(-kp)*ts*sin_tabn[index];
          if (theta<9*pi6)
          { 
          cmp1=0;               
          cmp2=(unsigned int)(ta/2+tb+tc);      
          cmp3=0;                
          cmp4=(unsigned int)(ta/2+tb);       
          cmp5=(unsigned int)(ta)>>1;         
          cmp6=(unsigned int)(ts2);         
          }
          else
          {
          cmp1=(unsigned int)(tc)>>1;               
          cmp2=(unsigned int)(ts2);      
          cmp3=0;                
          cmp4=(unsigned int)(ta+tb+tc/2);       
          cmp5=(unsigned int)(ta+tc/2);         
          cmp6=(unsigned int)(ts2); 
          }
         }
          else if((-volts)*sin_tabp[index]>vdcbeta)
          {
          triangle=2;
          ta=ts*(1+kp*sin_tab[index]);
          tb=(-kp)*ts*sin_tabn[index];
          tc=(-ts2)*(1+2*kp*sin_tabp[index]);
          cmp1=0;                
          cmp2=(unsigned int)(ta/2+tb);       
          cmp3=0;                
          cmp4=(unsigned int)(ta)>>1;          
          cmp5=(unsigned int)(ta/2+tb+tc);         
          cmp6=(unsigned int)(ts2);         
          }
          else if((-volts)*sin_tabn[index]>vdcbeta)
         {
          triangle=4;
          ta=(-ts2)*(1+2*kp*sin_tabn[index]);
          tb=(-kp)*ts*sin_tabp[index];
          tc=ts*(1+kp*sin_tab[index]);
          cmp1=(unsigned int)(tc/2+ta);         
          cmp2=(unsigned int)(ts2);           
          cmp3=0;                  
          cmp4=(unsigned int)(tc)>>1;            
          cmp5=(unsigned int)(tc/2+ta+tb);          
          cmp6=(unsigned int)(ts2);           
           } 
           else 
          {
          triangle=3;
          ta=(ts2)*(1+2*kp*sin_tabn[index]);
          tb=(-ts2)*(1+2*kp*sin_tab[index]);
          tc=(ts2)*(1+2*kp*sin_tabp[index]);
          if (theta<9*pi6)
          { 
           cmp1=0;                    
           cmp2=(unsigned int)(ta/2+tb+tc);       
           cmp3=0;                  
           cmp4=(unsigned int)(ta)>>1;             
           cmp5=(unsigned int)(ta/2+tb);               
           cmp6=(unsigned int)(ts2);            
          }
          else
          {
           cmp1=(unsigned int)(tc)>>1;                    
           cmp2=(unsigned int)(ts2);       
           cmp3=0;                  
           cmp4=(unsigned int)(ta+tc/2);             
           cmp5=(unsigned int)(ta+tb+tc/2);               
           cmp6=(unsigned int)(ts2);          
          } 
       }       
    }
/****************************************************************************/
/***********************************sector6**********************************/
/****************************************************************************/
else 
{       
       index=(unsigned int)(theta*40.744);
        if ((-volts)*sin_tabn[index]<=vdcbeta)
        {
          triangle=1;
          ta=(-kp)*ts*sin_tab[index];
          tb=(ts2)*(1+2*kp*sin_tabn[index]);
          tc=kp*ts*sin_tabp[index];
          if(theta<11*pi6)
          {
          cmp1=(unsigned int)(ta/2+tc);      
          cmp2=(unsigned int)(ts2);         
          cmp3=0;                
          cmp4=(unsigned int)(ta/2+tb+tc);    
          cmp5=(unsigned int)(ta)>>1;         
          cmp6=(unsigned int)(ts2);         
          }
          else
          {
          cmp1=(unsigned int)(tc)>>1;      
          cmp2=(unsigned int)(ts2);         
          cmp3=0;                
          cmp4=(unsigned int)(tb+tc/2);    
          cmp5=0;         
          cmp6=(unsigned int)(ta+tb+tc/2);         
          }
         } 
          else if((-volts)*sin_tab[index]>vdcbeta)
          {
          triangle=2;
          ta=ts*(1+kp*sin_tabn[index]);
          tb=kp*ts*sin_tabp[index];
          tc=(-ts2)*(1+2*kp*sin_tab[index]);
          cmp1=(unsigned int)(ta/2+tb+tc);        
          cmp2=(unsigned int)(ts2);            
          cmp3=0;                   
          cmp4=(unsigned int)(ta)>>1;             
          cmp5=(unsigned int)(ta/2+tc);               
          cmp6=(unsigned int)(ts2);            
          }
          else if(volts*sin_tabp[index]>vdcbeta)
         {
          triangle=4;
          ta=(-ts2)*(1-2*kp*sin_tabp[index]);
          tb=(-kp)*ts*sin_tab[index];
          tc=ts*(1+kp*sin_tabn[index]);
          cmp1=(unsigned int)(tc/2+ta+tb);       
          cmp2=(unsigned int)(ts2);            
          cmp3=0;                   
          cmp4=(unsigned int)(tc)>>1;             
          cmp5=0;                       
          cmp6=(unsigned int)(tc/2+tb);          
         } 
         else 
          {
          triangle=3;
          ta=(ts2)*(1-2*kp*sin_tabp[index]);
          tb=(ts2)*(2*(-kp)*sin_tabn[index]-1);
          tc=(ts2)*(1+2*kp*sin_tab[index]);
          if (theta<11*pi6)
          {
          cmp1=(unsigned int)(ta/2+tb+tc);       
          cmp2=(unsigned int)(ts2);           
          cmp3=0;                   
          cmp4=(unsigned int)(ta/2+tc);          
          cmp5=(unsigned int)(ta)>>1;                  
          cmp6=(unsigned int)(ts2);           
          }
          else
          { 
          cmp1=(unsigned int)(tb+tc/2);       
          cmp2=(unsigned int)(ts2);           
          cmp3=0;                   
          cmp4=(unsigned int)(tc)>>1;          
          cmp5=0;                  
          cmp6=(unsigned int)(ta+tb+tc/2); 
          }
        }    
     } /*end of sector 6*/
   *CMPR1=cmp1;
   *CMPR2=cmp2;
   *CMPR3=cmp3;
   *CMPR4=cmp4;
   *CMPR5=cmp5;
   *CMPR6=cmp6; 
   theta=theta+delta_theta;
   if(theta>=2*pi)theta=0;
   /*if(theta>=2*pi)theta=0;*/
   asm(" CLRC INTM");
 }  /*end of tmr2_isr*/
main()
{
	disable();					    
    initial();	                    
    initial2(); 
    qep_init();
    init_adc();
    para_init();
    enable();
    ADSOC();                      
    while(1)                         
    { 
    ;
    } 
}