dsp2812 pmsm foc之速度環電流環

//=================速度環PI===================================

            Speed_Ref=_IQ(SpeedRef);
			Speed_Fdb=Speed;

			Speed_Error=Speed_Ref - Speed_Fdb;//誤差

			Speed_Up=_IQmpy(Speed_Kp,Speed_Error);//kp
			

			Speed_OutPreSat=Speed_Up+Speed_Ui;

			if(Speed_OutPreSat>Speed_OutMax)
				{
                    Speed_Out=Speed_OutMax;

            }
			else if(Speed_OutPreSat<Speed_OutMin)
	 			{
                    Speed_Out=Speed_OutMin;
			}
			else 
				{
                    Speed_Out=Speed_OutPreSat;  
			}
	
			Speed_SatError=Speed_Out-Speed_OutPreSat;  
            Speed_Ui=Speed_Ui + _IQmpy(Speed_Ki,Speed_Up) + _IQmpy(Speed_Ki,Speed_SatError);

			IQ_Given=Speed_Out;                   // 速度與電流是線性的所以使用標幺值方便。雙環串聯
		} 
	    else 
            {
                SpeedLoopCount++; 
	    }

//======================================================================================================
//IQ電流PID調節控制
//======================================================================================================  
		IQ_Ref=IQ_Given;
		IQ_Fdb=iq;

		IQ_Error=IQ_Ref-IQ_Fdb;//誤差

		IQ_Up=_IQmpy(IQ_Kp,IQ_Error);//kp
		

		IQ_OutPreSat=IQ_Up+IQ_Ui;

		if(IQ_OutPreSat>IQ_OutMax)
			{
                IQ_Out=IQ_OutMax;
		}
		else if(IQ_OutPreSat<IQ_OutMin)
		 	{
                IQ_Out=IQ_OutMin;
		}
		else 
			{
                IQ_Out=IQ_OutPreSat;  
              }

		IQ_SatError=IQ_Out-IQ_OutPreSat;  
        IQ_Ui=IQ_Ui + _IQmpy(IQ_Ki,IQ_Up) + _IQmpy(IQ_Ki,IQ_SatError);

		Uq=IQ_Out;

//======================================================================================================
//ID電流PID調節控制
//======================================================================================================  
		ID_Ref=0;
		ID_Fdb=id;

		ID_Error=ID_Ref-ID_Fdb;//誤差

		ID_Up=_IQmpy(ID_Kp,ID_Error);    
		  

		ID_OutPreSat=ID_Up+ID_Ui;   

		if(ID_OutPreSat>ID_OutMax)   
			{
                ID_Out=ID_OutMax;
		}
		else if(ID_OutPreSat<ID_OutMin)
		 	{
                ID_Out=ID_OutMin;
                }
		else 
			{
                ID_Out=ID_OutPreSat; 
		}

		ID_SatError=ID_Out-ID_OutPreSat;  
        ID_Ui=ID_Ui+_IQmpy(ID_Ki,ID_Up)+_IQmpy(ID_Ki,ID_SatError);  

		Ud=ID_Out;