dsp2812 pmsm foc之定位結束，開始閉環控制

//======================================================================================================
//初始位置定位結束，開始閉環控制
//====================================================================================================== 
	else if(LocationFlag==LocationEnd)
	{  
		l++;   
        //旋轉方向判定
		DirectionQep = 0x4000&EvaRegs.GPTCONA.all;
		DirectionQep = DirectionQep>>14;
        RawTheta = _IQ(EvaRegs.T2CNT);
		RawTheta =TotalPulse - RawTheta;
        
// 計算機械角度
		if(DirectionQep ==1)                                     //T2STAT=1,遞增計數，代表順時針；
		{
            if((RawTheta> RawCnt1) && (OldRawThetaPos<_IQ(1000)))
			{
				PosCount += TotalCnt;                      //位置環先不管
			}
			Place_now= _IQtoF(TotalPulse - RawTheta)+PosCount; //位置環先不管
			OldRawThetaPos = RawTheta;                          //將當前角度計數賦到舊的上面去
		}
		else if(DirectionQep ==0)                                 //T2STAT=0，遞減計數，代表逆時針
		{
                if((OldRawThetaPos> RawCnt2) && (RawTheta<_IQ(900)))
			{
				PosCount -= TotalCnt;                     //位置環先不管
			}
			Place_now = _IQtoF(TotalPulse - RawTheta)+PosCount;     //位置環先不管
			OldRawThetaPos = RawTheta;                              //將當前角度計數賦到舊的上面去
		} 
		MechTheta = _IQmpy(MechScaler,RawTheta);                    //計算機械角度 見函數分析那一節
        if(MechTheta>_IQ(360))                                             //確保在-360到360度之間
        {MechTheta=MechTheta-_IQ(360);}
         if(MechTheta<_IQ(-360))
        {MechTheta=MechTheta+_IQ(360);}
		ElecTheta = _IQmpy(PolePairs,MechTheta);                     //電氣角度=機械角度乘以極對數
	
		AnglePU=_IQdiv(ElecTheta,_IQ(360))+Offset_Angle;             //標幺值角度  Offset_Angle是光電編碼器a相與z相的偏移角度。
	   	Sine = _IQsinPU(AnglePU);                                  // _IQsinPU(A)         sin(A*6.283185307)
	   	Cosine = _IQcosPU(AnglePU);