QEP速度计算

maxthonzwq

QEP速度计算

1. 
   
2. //======================================================================================================
   
3. //QEP速度计算
   
4. //======================================================================================================
   
5.    
   
6.      if (SpeedLoopCount>=SpeedLoopPrescaler)
   
7.      {   
   
8. // 旋转方向判定
   
9.    DirectionQep = 0x4000&EvaRegs.GPTCONA.all;
   
10.    DirectionQep = DirectionQep>>14;//0x4000=0100 0000 0000 0000故右移14位
    
11.     NewRawTheta =_IQ(EvaRegs.T2CNT);
    
12. // 计算机械角度
    
13.    if(DirectionQep ==1) //T2STAT=1,递增计数，代表反转；
    
14.    {
    
15.     RawThetaTmp = NewRawTheta - OldRawTheta;     
    
16.    }
    
17.    else if(DirectionQep ==0) //T2STAT=0，递减计数，代表正转
    
18.    {
    
19.      RawThetaTmp = OldRawTheta-NewRawTheta;
    
20.    }
    
21.    if(RawThetaTmp < _IQ(0))
    
22.      RawThetaTmp = RawThetaTmp + TotalPulse;
    
23.    else RawThetaTmp = RawThetaTmp;
    
24.    Speed = _IQmpy(RawThetaTmp,SpeedScaler);
    
25.    SpeedRpm = _IQmpy(BaseRpm,Speed);      
    
26.    OldRawTheta = NewRawTheta;
    
27.       SpeedLoopCount=1;
    
28.    RawThetaTmp=0;
    
29.   }
    
30.      else SpeedLoopCount++;
    

复制代码