aci.c

伺服控制数学模型

/*=====================================================================================
 File name:        ACI.C  (IQ version)                  
                    
 Originator:	Digital Control Systems Group
			Texas Instruments

 Description:  Emulated Induction Motor in the Stationary Reference Frame                 

=====================================================================================
 History:
-------------------------------------------------------------------------------------
 04-15-2005	Version 3.20
-------------------------------------------------------------------------------------*/

#include "IQmathLib.h"         // Include header for IQmath library
// Don't forget to set a proper GLOBAL_Q in "IQmathLib.h" file
#include "dmctype.h"
#include "aci.h"

void aci_calc(ACI *v)
{	

	_iq PredictedPsiAlpha, PredictedPsiBeta, PredictedIalpha, PredictedIbeta;
	_iq DPredictedPsiAlpha, DPredictedPsiBeta, DPredictedIalpha, DPredictedIbeta;
	_iq DPsiAlpha, DPsiBeta, DIalpha, DIbeta;
	_iq PredictedWr, DPredictedWr, DWr;
					
// Rotor flux/Stator current calculation
   // Predictor
   PredictedPsiBeta = v->PsiBeta - _IQmpy(v->K1,v->PsiBeta) + _IQmpy(_IQmpy(v->K2,v->Wr),v->PsiAlpha) + _IQmpy(v->K3,v->Ibeta);  
   PredictedPsiAlpha = v->PsiAlpha - _IQmpy(v->K1,v->PsiAlpha) - _IQmpy(_IQmpy(v->K2,v->Wr),v->PsiBeta) + _IQmpy(v->K3,v->Ialpha);
   PredictedIbeta = v->Ibeta + _IQmpy(v->K4,v->PsiBeta) - _IQmpy(_IQmpy(v->K5,v->Wr),v->PsiAlpha) - _IQmpy(v->K6,v->Ibeta) + _IQmpy(v->K7,v->Ubeta);  
   PredictedIalpha = v->Ialpha + _IQmpy(v->K4,v->PsiAlpha) + _IQmpy(_IQmpy(v->K5,v->Wr),v->PsiBeta) - _IQmpy(v->K6,v->Ialpha) + _IQmpy(v->K7,v->Ualpha);

   // Corrector 
   DPredictedPsiBeta = - _IQmpy(v->K1,PredictedPsiBeta) + _IQmpy(_IQmpy(v->K2,v->Wr),PredictedPsiAlpha) + _IQmpy(v->K3,PredictedIbeta);  
   DPredictedPsiAlpha = - _IQmpy(v->K1,PredictedPsiAlpha) - _IQmpy(_IQmpy(v->K2,v->Wr),PredictedPsiBeta) + _IQmpy(v->K3,PredictedIalpha);
   DPredictedIbeta = _IQmpy(v->K4,PredictedPsiBeta) - _IQmpy(_IQmpy(v->K5,v->Wr),PredictedPsiAlpha) - _IQmpy(v->K6,PredictedIbeta) + _IQmpy(v->K7,v->Ubeta);  
   DPredictedIalpha = _IQmpy(v->K4,PredictedPsiAlpha) + _IQmpy(_IQmpy(v->K5,v->Wr),PredictedPsiBeta) - _IQmpy(v->K6,PredictedIalpha) + _IQmpy(v->K7,v->Ualpha);
  
   DPsiBeta = PredictedPsiBeta - v->PsiBeta;  
   DPsiAlpha = PredictedPsiAlpha - v->PsiAlpha;  
   DIbeta = PredictedIbeta - v->Ibeta;  
   DIalpha = PredictedIalpha - v->Ialpha; 

   v->PsiBeta = v->PsiBeta + _IQmpy(_IQ(0.5),(_IQmpy((_IQ(1)+v->Alpha),DPredictedPsiBeta) + _IQmpy((_IQ(1)-v->Alpha),DPsiBeta)));
   v->PsiAlpha = v->PsiAlpha + _IQmpy(_IQ(0.5),(_IQmpy((_IQ(1)+v->Alpha),DPredictedPsiAlpha) + _IQmpy((_IQ(1)-v->Alpha),DPsiAlpha)));
   
   v->Ibeta = v->Ibeta + _IQmpy(_IQ(0.5),(_IQmpy((_IQ(1)+v->Alpha),DPredictedIbeta) + _IQmpy((_IQ(1)-v->Alpha),DIbeta)));
   v->Ialpha = v->Ialpha + _IQmpy(_IQ(0.5),(_IQmpy((_IQ(1)+v->Alpha),DPredictedIalpha) + _IQmpy((_IQ(1)-v->Alpha),DIalpha)));
    
// Electromagnetic Torque calculation
   v->Torque = _IQmpy(v->K8,(_IQmpy(v->PsiAlpha,v->Ibeta) - _IQmpy(v->PsiBeta,v->Ialpha)));
   
// Rotor speed calculation 
   PredictedWr = v->Wr - _IQmpy(v->K9,v->Wr) + _IQmpy(v->K10,(v->Torque - v->LoadTorque));

   DPredictedWr = - _IQmpy(v->K9,PredictedWr) + _IQmpy(v->K10,(v->Torque - v->LoadTorque));
   
   DWr = PredictedWr - v->Wr;
   // DWr = - v->K9*v->Wr + v->K10*(v->Torque - v->LoadTorque); 

   v->Wr = v->Wr + _IQmpy(_IQ(0.5),(_IQmpy((_IQ(1)+v->Alpha),DPredictedWr) + _IQmpy((_IQ(1)-v->Alpha),DWr)));

// Q0 = Q0*GLOBAL_Q => _IQXmpy(), X = GLOBAL_Q
   v->WrRpm = _IQmpy(v->Wr,v->BaseRpm); 

}