fuzzzy.c

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

   } 
   else
    {
     cmp1=0;         
     cmp2=(unsigned int)(ta+tb+n*tc);         
     cmp3=(unsigned int)(tb+n*tc);    
     cmp4=(unsigned int)ts2;         
     cmp5=0;                 
     cmp6=(unsigned int)(n*tc);
     }  
   }
 }
/******************************3 sector*****************************/
else if ((ceta>=2*pi3)&&(ceta<pi)) 
{ 
  
    if (volts*sin_tabn[i]<=vdcbeta)
    {
      triangle=1;
      ta=kp*ts*sin_tab[i];
      tb=ts2*(1-2*kp*sin_tabn[i]);
      tc=(-kp)*ts*sin_tabp[i]; 
      if (ceta<5*pi6)
      {
        cmp1=0;                
        cmp2=(unsigned int)(m*ta+tb);      
        cmp3=(unsigned int)(m*ta);          
        cmp4=(unsigned int)ts2;        
        cmp5=0;               
        cmp6=(unsigned int)(m*ta+tb+tc);  
       } 
      else
       {
        cmp1=0;                
        cmp2=(unsigned int)(ta+tb+n*tc);      
        cmp3=(unsigned int)(ta+n*tc);          
        cmp4=(unsigned int)ts2;        
        cmp5=(unsigned int)(n*tc);               
        cmp6=(unsigned int)ts2;
        }
      } 
      else if(volts*sin_tab[i]>vdcbeta)
       {
      triangle=2;
      ta=ts*(1-kp*sin_tabn[i]);
      tb=(-kp)*ts*sin_tabp[i];
      tc=ts2*(2*kp*sin_tab[i]-1);
      cmp1=0;                
      cmp2=(unsigned int)(m*ta);         
      cmp3=(unsigned int)(m*ta+tb+tc);   
      cmp4=(unsigned int)ts2;         
      cmp5=0;                
      cmp6=(unsigned int)(m*ta+tb);      
     }
   else if((-volts)*sin_tabp[i]>vdcbeta)
     {
       triangle=4;
       ta=(-ts2)*(1+2*kp*sin_tabp[i]);
       tb=kp*ts*sin_tab[i];
       tc=ts*(1-kp*sin_tabn[i]);
       cmp1=0;                
       cmp2=(unsigned int)(n*tc);        
       cmp3=(unsigned int)(n*tc+ta+tb);    
       cmp4=(unsigned int)ts2;         
       cmp5=(unsigned int)(n*tc+ta);          
       cmp6=(unsigned int)ts2;
       }        
    else 
    {
       triangle=3;
       ta=ts2*(1+2*kp*sin_tabp[i]);
       tb=ts2*(2*kp*sin_tabn[i]-1);
       tc=ts2*(1-2*kp*sin_tab[i]);
        if(ceta<5*pi6) 
        {
        cmp1=0;                 
        cmp2=(unsigned int)(m*ta);          
        cmp3=(unsigned int)(m*ta+tb);       
        cmp4=(unsigned int)ts2;        
        cmp5=0;                
        cmp6=(unsigned int)(m*ta+tb+tc);    
        } 
       else
        {
        cmp1=0;                 
        cmp2=(unsigned int)(ta+n*tc);          
        cmp3=(unsigned int)(ta+n*tc+tb);       
        cmp4=(unsigned int)ts2;        
        cmp5=(unsigned int)(n*tc);                 
        cmp6=(unsigned int)ts2;  
        } 
    }   
}
/********************************************/
/*4 sector*/
/********************************************/
else if((ceta>=pi)&&(ceta<4*pi3)) 
{
   if ((-volts)*sin_tabp[i]<=vdcbeta)
     {
      triangle=1;
      ta=kp*ts*sin_tabn[i];
      tb=ts2*(1+2*kp*sin_tabp[i]);
      tc=(-kp)*ts*sin_tab[i]; 
      if (ceta<=7*pi6) 
      {
       cmp1=0;                   
       cmp2=(unsigned int)(m*ta+tb+tc);       
       cmp3=(unsigned int)(m*ta);             
       cmp4=(unsigned int)ts2;             
       cmp5=(unsigned int)(m*ta+tc);           
       cmp6=(unsigned int)ts2;             
      } 
      else
      { 
       cmp1=0;                   
       cmp2=(unsigned int)(tb+n*tc);       
       cmp3=0;             
       cmp4=(unsigned int)(ta+tb+n*tc);             
       cmp5=(unsigned int)(n*tc);           
       cmp6=(unsigned int)ts2;  
       }
     } 
      else if(volts*sin_tabn[i]>vdcbeta) 
      {
      triangle=2;
      ta=ts*(1+kp*sin_tabp[i]);
      tb=(-kp)*ts*sin_tab[i];
      tc=ts2*(2*kp*sin_tabn[i]-1);
      cmp1=0;                 
      cmp2=(unsigned int)(m*ta);          
      cmp3=(unsigned int)(m*ta+tc);       
      cmp4=(unsigned int)ts2;          
      cmp5=(unsigned int)(m*ta+tb+tc);         
      cmp6=(unsigned int)ts2;        
      }
     else if((-volts)*sin_tab[i]>vdcbeta)
       {
       triangle=4;
       ta=(-ts2)*(1+2*kp*sin_tab[i]);
       tb=kp*ts*sin_tabn[i];
       tc=ts*(1+kp*sin_tabp[i]);
       cmp1=0;                
       cmp2=(unsigned int)(n*tc);         
       cmp3=0;                
       cmp4=(unsigned int)(n*tc+tb);       
       cmp5=(unsigned int)(n*tc+ta+tb);       
       cmp6=(unsigned int)ts2;        
        }
      else 
      {
       triangle=3;
       ta=ts2*(1+2*kp*sin_tab[i]);
       tb=(-ts2)*(1+2*kp*sin_tabp[i]);
       tc=ts2*(1-2*kp*sin_tabn[i]);
       if (ceta<7*pi6)
       {
        cmp1=0;                 
        cmp2=(unsigned int)(m*ta+tc);        
        cmp3=(unsigned int)(m*ta);           
        cmp4=(unsigned int)ts2;          
        cmp5=(unsigned int)(m*ta+tb+tc);      
        cmp6=(unsigned int)ts2;         
       } 
       else
       { 
        cmp1=0;                 
        cmp2=(unsigned int)(n*tc);        
        cmp3=0;           
        cmp4=(unsigned int)(ta+tb+n*tc);          
        cmp5=(unsigned int)(tb+n*tc);      
        cmp6=(unsigned int)ts2;       
       }
    }                         
}
/**************************************************************************/
/***********************************sector5********************************/
/**************************************************************************/
else if((ceta>=4*pi3)&&(ceta<5*pi3)) 
{
         if ((-volts)*sin_tab[i]<=vdcbeta)
        {
          triangle=1;
          ta=(-kp)*ts*sin_tabp[i];
          tb=ts2*(1+2*kp*sin_tab[i]);
          tc=(-kp)*ts*sin_tabn[i];
          if (ceta<3*pi2)
          { 
          cmp1=0;               
          cmp2=(unsigned int)(m*ta+tb+tc);      
          cmp3=0;                
          cmp4=(unsigned int)(m*ta+tb);       
          cmp5=(unsigned int)(m*ta);         
          cmp6=(unsigned int)ts2;         
          }
          else
          {
          cmp1=(unsigned int)(n*tc);               
          cmp2=(unsigned int)ts2;      
          cmp3=0;                
          cmp4=(unsigned int)(ta+tb+n*tc);       
          cmp5=(unsigned int)(ta+n*tc);         
          cmp6=(unsigned int)ts2; 
          }
         }
          else if((-volts)*sin_tabp[i]>vdcbeta)
          {
          triangle=2;
          ta=ts*(1+kp*sin_tab[i]);
          tb=(-kp)*ts*sin_tabn[i];
          tc=(-ts2)*(1+2*kp*sin_tabp[i]);
          cmp1=0;                
          cmp2=(unsigned int)(m*ta+tb);       
          cmp3=0;                
          cmp4=(unsigned int)(m*ta);          
          cmp5=(unsigned int)(m*ta+tb+tc);         
          cmp6=(unsigned int)ts2;         
          }
          else if((-volts)*sin_tabn[i]>vdcbeta)
         {
          triangle=4;
          ta=(-ts2)*(1+2*kp*sin_tabn[i]);
          tb=(-kp)*ts*sin_tabp[i];
          tc=ts*(1+kp*sin_tab[i]);
          cmp1=(unsigned int)(n*tc+ta);         
          cmp2=(unsigned int)ts2;           
          cmp3=0;                  
          cmp4=(unsigned int)(n*tc);            
          cmp5=(unsigned int)(n*tc+ta+tb);          
          cmp6=(unsigned int)ts2;           
           } 
           else 
          {
          triangle=3;
          ta=ts2*(1+2*kp*sin_tabn[i]);
          tb=(-ts2)*(1+2*kp*sin_tab[i]);
          tc=ts2*(1+2*kp*sin_tabp[i]);
          if (ceta<3*pi2)
          { 
           cmp1=0;                    
           cmp2=(unsigned int)(m*ta+tb+tc);       
           cmp3=0;                  
           cmp4=(unsigned int)(m*ta);             
           cmp5=(unsigned int)(m*ta+tb);               
           cmp6=(unsigned int)ts2;            
          }
          else
          {
           cmp1=(unsigned int)(n*tc);                    
           cmp2=(unsigned int)ts2;       
           cmp3=0;                  
           cmp4=(unsigned int)(ta+n*tc);             
           cmp5=(unsigned int)(ta+tb+n*tc);               
           cmp6=(unsigned int)ts2;          
          } 
       }       
    }
/****************************************************************************/
/***********************************sector6**********************************/
/****************************************************************************/
else 
{
        if ((-volts)*sin_tabn[i]<=vdcbeta)
        {
          triangle=1;
          ta=(-kp)*ts*sin_tab[i];
          tb=ts2*(1+2*kp*sin_tabn[i]);
          tc=kp*ts*sin_tabp[i];
          if(ceta<11*pi6)
          {
          cmp1=(unsigned int)(m*ta+tc);      
          cmp2=(unsigned int)ts2;         
          cmp3=0;                
          cmp4=(unsigned int)(m*ta+tb+tc);    
          cmp5=(unsigned int)(m*ta);         
          cmp6=(unsigned int)ts2;         
          }
          else
          {
          cmp1=(unsigned int)(n*tc);      
          cmp2=(unsigned int)ts2;         
          cmp3=0;                
          cmp4=(unsigned int)(tb+n*tc);    
          cmp5=0;         
          cmp6=(unsigned int)(ta+tb+n*tc);         
          }
         } 
          else if((-volts)*sin_tab[i]>vdcbeta)
          {
          triangle=2;
          ta=ts*(1+kp*sin_tabn[i]);
          tb=kp*ts*sin_tabp[i];
          tc=(-ts2)*(1+2*kp*sin_tab[i]);
          cmp1=(unsigned int)(m*ta+tb+tc);        
          cmp2=(unsigned int)ts2;            
          cmp3=0;                   
          cmp4=(unsigned int)(m*ta);             
          cmp5=(unsigned int)(m*ta+tc);               
          cmp6=(unsigned int)ts2;            
          }
          else if(volts*sin_tabp[i]>vdcbeta)
         {
          triangle=4;
          ta=(-ts2)*(1-2*kp*sin_tabp[i]);
          tb=(-kp)*ts*sin_tab[i];
          tc=ts*(1+kp*sin_tabn[i]);
          cmp1=(unsigned int)(n*tc+ta+tb);       
          cmp2=(unsigned int)ts2;            
          cmp3=0;                   
          cmp4=(unsigned int)(n*tc);             
          cmp5=0;                       
          cmp6=(unsigned int)(n*tc+tb);          
         } 
         else 
          {
          triangle=3;
          ta=ts2*(1-2*kp*sin_tabp[i]);
          tb=ts2*(2*(-kp)*sin_tabn[i]-1);
          tc=ts2*(1+2*kp*sin_tab[i]);
          if (ceta<11*pi6)
          {
          cmp1=(unsigned int)(m*ta+tb+tc);       
          cmp2=(unsigned int)ts2;           
          cmp3=0;                   
          cmp4=(unsigned int)(m*ta+tc);          
          cmp5=(unsigned int)(m*ta);                  
          cmp6=(unsigned int)ts2;           
          }
          else
          { 
          cmp1=(unsigned int)(tb+n*tc);       
          cmp2=(unsigned int)ts2;           
          cmp3=0;                   
          cmp4=(unsigned int)(n*tc);          
          cmp5=0;                  
          cmp6=(unsigned int)(ta+tb+n*tc); 
          }
        }    
     } /*end of sector 6*/
   *CMPR1=cmp1;
   *CMPR2=cmp2;
   *CMPR3=cmp3;
   *CMPR4=cmp4;
   *CMPR5=cmp5;
   *CMPR6=cmp6; 
   i++; 
   if(i>=60)i=0;
   asm(" CLRC INTM");
 }  
/******************main program**********************/
main()
{
	disable();					    
    dsp_init();
    peri_init();
    /*qep_init();*/
    init_adc();
    enable();
    ADSOC();                     
    while(1)                         
    { 
    ;
    } 
}