电机圈首页 › 〖电机控制技术〗 › QEP速度计算

maxthonzwq 发表于 2012-5-7 14:22

QEP速度计算

QEP速度计算

//======================================================================================================
//QEP速度计算
//======================================================================================================
   
   if (SpeedLoopCount>=SpeedLoopPrescaler)
   {   
// 旋转方向判定
   DirectionQep = 0x4000&EvaRegs.GPTCONA.all;
   DirectionQep = DirectionQep>>14;//0x4000=0100 0000 0000 0000故右移14位
    NewRawTheta =_IQ(EvaRegs.T2CNT);
// 计算机械角度
   if(DirectionQep ==1) //T2STAT=1,递增计数，代表反转；
   {
    RawThetaTmp = NewRawTheta - OldRawTheta;   
   }
   else if(DirectionQep ==0) //T2STAT=0，递减计数，代表正转
   {
   RawThetaTmp = OldRawTheta-NewRawTheta;
   }
   if(RawThetaTmp < _IQ(0))
   RawThetaTmp = RawThetaTmp + TotalPulse;
   else RawThetaTmp = RawThetaTmp;
   Speed = _IQmpy(RawThetaTmp,SpeedScaler);
   SpeedRpm = _IQmpy(BaseRpm,Speed);      
   OldRawTheta = NewRawTheta;
      SpeedLoopCount=1;
   RawThetaTmp=0;
}
   else SpeedLoopCount++;

查看完整版本: QEP速度计算