idc.c

这是英飞凌公司基于xc166开发的foc方式控制的pmsm电机例程 非常有参加价值

#include "MAIN.H"
#include "DEF.H"
#include "TABLES.H"
#include "TableQ15.h"

extern void SVM(TComplex *m, char *Sector);
extern void PI_Controller(TPI * PI, int NewInput,int * Result);

void Clarke_Park_Trans(TPhaseCurrent * In, TComplex * angle, TComplex * Result);
void Park_Rev(TComplex * Frame_dq, TComplex * angle, TComplex  * Result);
void InitPIController(TPI * Data, int Kp, int Ki,int Limit);

const int T_sample = 0x000D;  /* 400us  Q1.15 */

char Sector;
int period_number=1;
int count=0;
int QuadratureCurrent = 0x0;
int DirectCurrent  = 0x0;
int RotorSpeedSet = 0x0;
int RotorSpeed;
int oldposition;
int deltaposition;
int deltagammaQ15;	
int omega;

unsigned int decoval;
unsigned long int decoder_wert;

TPhaseCurrent PhaseCurrent;
TPhaseCurrent PhaseCurrentQ15;
TComplex Gamma;
TComplex RotorCurrentQ15;
TComplex PIRotorCurrentQ15;
TComplex StatorCurrentQ15;

TPI PI_Data_Id;		// PI Current Direct
TPI PI_Data_Iq;		// PI Current Quadrature
TPI PI_Data_Speed;	// PI control speed

//****************************************************************************
// @Function      void InitPIController(TPI * Data, int Kp, int Ki,int Limit)
//
//----------------------------------------------------------------------------
// @Description   Initialation for PI Controller 
//				  	
//----------------------------------------------------------------------------
// @Returnvalue   none
//
//----------------------------------------------------------------------------
// @Parameters    TPI *Data
//				  int Kp 
//				  int Ki 
//				  int Limit 
//
//----------------------------------------------------------------------------
// @Date          01.10.2003
//
//****************************************************************************
void InitPIController(TPI * Data, int Kp, int Ki,int Limit)
{	
	Data->SetValue = 0;
	Data->A0 = Kp;
	Data->A1 = ((((long)Ki << 4)*(long)T_sample) >> 15) - Kp; /* Q16.0 format */
	Data->Limit = Limit;
	Data->Integral = 0;
	Data->Error = 0;
}

//****************************************************************************
// @Function      void InitFOC (void) 
//
//----------------------------------------------------------------------------
// @Description   This Function is makeing settings for PI-Controller 
//
//
//----------------------------------------------------------------------------
// @Returnvalue   None
//
//----------------------------------------------------------------------------
// @Parameters    int Sector
//
//----------------------------------------------------------------------------
// @Date          01.10.2003
//
//****************************************************************************
void InitFOC(void)
{	
	InitPIController(&PI_Data_Speed, 0x3000, 0x0020, 0x5000);
	InitPIController(&PI_Data_Id, 0x1200, 0x0080,0x5000);
	InitPIController(&PI_Data_Iq, 0x1200, 0x0080,0x5000);

	DirectCurrent	=	0x0000;		// no field-weaking
	QuadratureCurrent=	0x0000;
}



//****************************************************************************
// @Function      void ClarkeParkTrans (TPhaseCurrent * In, TComplex * angle, TComplex * Result) 
//
//----------------------------------------------------------------------------
// @Description   Implemants the Clark and Park Transformation 
//				  (Three Phase Frame -> Rotor Frame Transformation)
//				  Execution time 2.2us @ 40MHz	
//----------------------------------------------------------------------------
// @Returnvalue   Tcomplex *Result (Current in rotating Frame)
//
//----------------------------------------------------------------------------
// @Parameters    TPhaseCurrent *In (Current in 3 Phases frame)
//				  Tcomplex *angle 
//
//----------------------------------------------------------------------------
// @Date          04.10.2003
//
//****************************************************************************
void ClarkeParkTrans(TPhaseCurrent *In, TComplex *angle, TComplex *Result)
{
	int Ibeta;
	TComplex IsPark;

	Ibeta = ((long)In->iv - (long)In->iw)*(long)SQRT3INV >> 15;
	IsPark.real = ((long)In->iu)*((long)angle->real) + ((long)Ibeta)*((long)angle->imag) >> 15;
	IsPark.imag = ((long)Ibeta)*((long)angle->real) - ((long)In->iu)*((long)angle->imag) >> 15;

	* Result =IsPark; 
 
}

//****************************************************************************
// @Function      void ParkInverse (TComplex *Frame_dq, TComplex *angle, TComplex  *Result) 
//
//----------------------------------------------------------------------------
// @Description   Implemants the reverse Park Transformation 
//				  (Rotor Frame -> Stator Frame Transformation)
//				  Execution time 1.45us @ 40MHz	
//----------------------------------------------------------------------------
// @Returnvalue   Tcomplex *Result (Current in Stator Frame)
//
//----------------------------------------------------------------------------
// @Parameters    Tcomplex *Frame_dq (Current in rotating frame)
//				  Tcomplex *angle 
//
//----------------------------------------------------------------------------
// @Date          04.10.2003
//
//****************************************************************************
void ParkInverse (TComplex *Frame_dq, TComplex *angle, TComplex  *Result)
{
	TComplex Is_rev_Park;

	Is_rev_Park.real = ((long)Frame_dq->real)*((long)angle->real) - ((long)Frame_dq->imag)*((long)angle->imag) >> 15;
	Is_rev_Park.imag = ((long)Frame_dq->real)*((long)angle->imag) + ((long)Frame_dq->imag)*((long)angle->real) >> 15;

	*Result=Is_rev_Park;
 
}

//****************************************************************************
// @Function      void GetRotorPosition (void) 
//
//----------------------------------------------------------------------------
// @Description   This Function calculates the actual position of the Rotor of 
//				  the PMSM in degrees	
//				  Execution time 1.8us @ 40MHz	
//
//----------------------------------------------------------------------------
// @Returnvalue     
//
//----------------------------------------------------------------------------
// @Parameters    None
//
//----------------------------------------------------------------------------
// @Globals		  
//
//----------------------------------------------------------------------------
// @Date          03.11.2003
//
//****************************************************************************
int GetRotorPosition(void)
{
	int decoval;

	decoval=GPT12E_T3;		
	if(!(decoval>=0&&decoval<=2000))		
		decoval=decoval-0xF830;			

	decoval=decoval%1000;
	decoval=decoval+ENC_OFFSET;	
	
	if (decoval>1000)
	{
		decoval=decoval-1000;
	}

	// Sinus and Cosine Function of Rotor Angle
	// for Park Transformation
	Gamma.imag=sin_table[decoval];
	Gamma.real=cos_table[decoval];
		
	return decoval;
}

//****************************************************************************
// @Function      void Iphases (void) 
//
//----------------------------------------------------------------------------
// @Description   Measure the phase currents with LEMs in phases 
//				  FUNCTION NOT USED WHEN MEASUREMENT IN DC LINK
//
//----------------------------------------------------------------------------
// @Returnvalue   None
//
//----------------------------------------------------------------------------
// @Parameters    None
//
//----------------------------------------------------------------------------
// @Date          04.11.2003
//
//****************************************************************************
/*
void Iphases (void) 
{
	int adciu;
	int adciv;

	// Set the ADC and start the conversion
	PECC0          =  0x0202;      	// load PECC0 control register
	DSTP0=_sof_(&adc_result[0]);
	ADC_CON_ADST   =  1;           	// set start bit for phases measurement

	adciu=adc_result[0]&0x03FF;		
	adciu=adciu-LEM_OFFSET;
	adciv=adc_result[1]&0x03FF;		
	adciv=adciv-LEM_OFFSET;
	
	if (adciu>0)
	{
		PhaseCurrentQ15.iu =ScaleQ15(adciu);
	}
	else
	{
		adciu=-adciu;
		PhaseCurrentQ15.iu =ScaleQ15Neg(adciu);
	}
	if (adciv>0)
	{
		PhaseCurrentQ15.iv =ScaleQ15(adciv);
	}
	else
	{
		adciv=-adciv;
		PhaseCurrentQ15.iv =ScaleQ15Neg(adciv);
	}
	PhaseCurrentQ15.iw = -PhaseCurrentQ15.iu - PhaseCurrentQ15.iv;
}
*/

//****************************************************************************
// @Function      void Idc_Iphases (void) 
//
//----------------------------------------------------------------------------
// @Description   This Function calculates the current in all 3 phases about 
//				  the current in the DC-link 
//				  Execution time 2.30us @ 40MHz	
//
//----------------------------------------------------------------------------
// @Returnvalue   None
//
//----------------------------------------------------------------------------
// @Parameters    int Sector
//
//----------------------------------------------------------------------------
// @Date          27.10.2003
//
//****************************************************************************

void Idc_Iphases (int Sector) 
{
	int adc0_10b;
	int adc1_10b;
	TPhaseCurrent CurrentQ15;
	
	adc0_10b=(adc_dclink_res[0]>>2);		// ADDAT2 is a 12 Bit value 
	adc1_10b=(adc_dclink_res[1]>>2);		// Conversion to a 10 Bit value and mult with 2 

	switch (Sector)
	{ 
	case 0:
		CurrentQ15.iu = ScaleQ15(adc1_10b);
		CurrentQ15.iw = ScaleQ15Neg(adc0_10b);
		CurrentQ15.iv = - CurrentQ15.iu - CurrentQ15.iw;
		break;	
	case 1:
		CurrentQ15.iv = ScaleQ15(adc1_10b);
		CurrentQ15.iw = ScaleQ15Neg(adc0_10b);
		CurrentQ15.iu = - CurrentQ15.iv - CurrentQ15.iw;
		break;
	case 2:
		CurrentQ15.iv = ScaleQ15(adc1_10b);
		CurrentQ15.iu = ScaleQ15Neg(adc0_10b);
		CurrentQ15.iw = -CurrentQ15.iv - CurrentQ15.iu;
		break;
	case 3:
		CurrentQ15.iw = ScaleQ15(adc1_10b);
		CurrentQ15.iu = ScaleQ15Neg(adc0_10b);
		CurrentQ15.iv = - CurrentQ15.iu - CurrentQ15.iw;
		break;
	case 4:	
		CurrentQ15.iw = ScaleQ15(adc1_10b);
		CurrentQ15.iv = ScaleQ15Neg(adc0_10b);
		CurrentQ15.iu = - CurrentQ15.iw - CurrentQ15.iv;
		break;
	case 5:
		CurrentQ15.iu = ScaleQ15(adc1_10b);
		CurrentQ15.iv = ScaleQ15Neg(adc0_10b);
		CurrentQ15.iw = -CurrentQ15.iu - CurrentQ15.iv;
		break;
	}

	// p.u. quantities: I=Imeasured/Iref; Iref=Inominal*sqrt2
	PhaseCurrentQ15.iu = ((long)CurrentQ15.iu)*((long)SQRT2INV) >> 15;	
	PhaseCurrentQ15.iv = ((long)CurrentQ15.iv)*((long)SQRT2INV) >> 15;
	PhaseCurrentQ15.iw = ((long)CurrentQ15.iw)*((long)SQRT2INV) >> 15;

	//  Show Current in Phases U, V or W (TEST)
	//	CC1_CC2 = 0xFFFF-(ShowQ15(PhaseCurrentQ15.iu));
	//	CC1_CC3 = 0xFFFF-(ShowQ15(PhaseCurrentQ15.iv));
	//	CC1_CC4 = 0xFFFF-(ShowQ15(PhaseCurrentQ15.iw));
}

//****************************************************************************
// @Function      int GetSpeed (int actpos) 
//
//----------------------------------------------------------------------------
// @Description   
//				  Execution time 2.15us @ 40MHz	
// 
//
//----------------------------------------------------------------------------
// @Returnvalue   int speed
//
//----------------------------------------------------------------------------
// @Parameters    int actpos
//
//----------------------------------------------------------------------------
// @Globals		  int deltagammaQ15
//
//----------------------------------------------------------------------------
// @Date          18.11.2003
//
//****************************************************************************
int GetSpeed(int actpos)
{
	int olddeltagamma;
	
	olddeltagamma=deltagammaQ15;

	deltaposition=actpos-oldposition;

	if (deltaposition>500) 	// overflow in left direction?
	{
		deltaposition= deltaposition-1000;
	}
	if (deltaposition < (-500))  // overflow in right direction?
	{
		deltaposition= deltaposition+1000;
	}

	if (deltaposition>=0)	// direction?
	{
		deltagammaQ15=(deltaposition & 0xFF)<<7;
	}
	else 
	{
		deltagammaQ15= 0x8000 + ((deltaposition & 0xFF)<<7);
	}
	omega=deltagammaQ15;

	deltagammaQ15=(((long)omega*(long)ONEOVER4)+((long)olddeltagamma*(long)THREEOVER4))>>15;

	oldposition=actpos;
	return deltagammaQ15;
}


//****************************************************************************
// @Function      void Pmsm_Foc (void) 
//
//----------------------------------------------------------------------------
// @Description   This Function is called upon each Period Match of Timer 12
// 
//
//----------------------------------------------------------------------------
// @Returnvalue   None
//
//----------------------------------------------------------------------------
// @Parameters    None
//
//----------------------------------------------------------------------------
// @Date          27.10.2003
//
//****************************************************************************
void Pmsm_Foc(void)
{
	int actpos;
	int position;
	
	switch (period_number)
	{
	case 1:
		Idc_Iphases(Sector);			// Get 3 phase currents from dc current
		position=GetRotorPosition();	// Get new rotor position
		if (count >= 22)				// execute the speed controller each 4.2ms
		{
			actpos=position;
			RotorSpeed=GetSpeed(actpos);
			if (dir_flag)
			{
				RotorSpeedSet=ScaleQ15(speed);
			}
			else
			{
				RotorSpeedSet=ScaleQ15Neg(speed);
			}
			PI_Data_Speed.SetValue = RotorSpeedSet;	
			PI_Controller(&PI_Data_Speed, RotorSpeed, &QuadratureCurrent);
			CC1_CC3 = 0xFFFF-(ShowQ15(RotorSpeed));	
			count=0;					
		}
		count++;
		if( RotorSpeedSet == 0 )
		{
			QuadratureCurrent = 0;
			DirectCurrent = 0;
		}
		// Execute the clarke & park transformation
		ClarkeParkTrans(&PhaseCurrentQ15, &Gamma ,&RotorCurrentQ15);

		period_number=2; 
	break;
		
	case 2: 
		// Execute the two PI controller for the current
		PI_Data_Id.SetValue = DirectCurrent;
		PI_Data_Iq.SetValue = QuadratureCurrent;
		PI_Controller(&PI_Data_Id, RotorCurrentQ15.real,&PIRotorCurrentQ15.real);								// 12,8祍
		PI_Controller(&PI_Data_Iq, RotorCurrentQ15.imag,&PIRotorCurrentQ15.imag);								// 12,8祍
		// Execute the reverse park transformation
		ParkInverse(&PIRotorCurrentQ15, &Gamma, &StatorCurrentQ15);
		SVM(&StatorCurrentQ15, &Sector);

		period_number=1; 
		break;

	default: period_number=1;
	}								
}