pi.c

为BLDC的电流环控制

/*
***********************************************************************
* For test BLDC
* 2008-1-7
***********************************************************************
*/
//=============================================//
//Include File
//=============================================//
#include	"Spmc75f2313a.h"
#include	"Spmc_typedef.h"
#include "config.h"

static PID sPID;
PID *sptr = &sPID;
static CONTROLSM sCtrlSM;
static CONTROLSM *sCSptr = &sCtrlSM;
UInt16 cnt[200];
UInt16 *cntptr=cnt;
UInt8  step;

UInt16 CountEn,Count;

static unsigned int uiFilter[FILTERLEN]={0};
static unsigned int *ptr = uiFilter;
static long summation = 0;

extern UInt16 adcnt;
extern UInt16 iav;
int motorposition = 1;
//=============================================//
//120度方波控制信号PWM加入方式选择及正反转选择
//=============================================//
#define		Turnaround		6
/*
	0 －－ 正转、上相PWM120度方波
	1 －－ 反转、上相PWM120度方波
	2 －－ 正转、下相PWM120度方波
	3 －－ 反转、下相PWM120度方波
	
	4 －－ 正转、前半相PWM120度方波
	5 －－ 反转、前半相PWM120度方波
	6 －－ 正转、后半相PWM120度方波
	7 －－ 反转、后半相PWM120度方波	
*/
//=============================================//
//=============================================//

/*
**********************************************************************
*
* PI init
*
**********************************************************************
*/

void DK_motor_init(void)
{
	sCSptr->B._direction = CCW;
	CountEn = DISABLE;
}
/*
**********************************************************************
*
* start pro
*
**********************************************************************
*/
void DK_start()
{
	
	IncPIDInit();
	sptr->SetPoint   = 256;
	sptr->LowRange = 200;
	sptr->HighRange = 500;
	sCSptr->B._startup = ENABLE;
	/*if(sCSptr->B._direction)
		BLDC_Test4_NU120FullPWM(P_POS1_DectData->W);
	else
		BLDC_Test4_PU120FullPWM(P_POS1_DectData->W);*/
	//P_TMR_Start->B.TMR4ST |= CB_TMR_TMR4ST_Start;
	

}

void DK_stop()
{
	sCSptr->B._startup = DISABLE;
	//P_TMR_Start->B.TMR4ST &= (~CB_TMR_TMR4ST_Start);
	P_TMR4_OutputCtrl->W = CW_TMR4_POLP_Active_High	|	\
						CW_TMR4_UPWM_Out_HL | CW_TMR4_UOC_Mode1 |\
						CW_TMR4_VPWM_Out_HL  | CW_TMR4_VOC_Mode1 |\
						CW_TMR4_WPWM_Out_HL  | CW_TMR4_WOC_Mode1;
	F_Delay(10);
	P_TMR4_OutputCtrl->W = CW_TMR4_POLP_Active_High	|	\
						CW_TMR4_UPWM_Out_HL | CW_TMR4_UOC_Mode0 |\
						CW_TMR4_VPWM_Out_HL  | CW_TMR4_VOC_Mode0 |\
						CW_TMR4_WPWM_Out_HL  | CW_TMR4_WOC_Mode0;
	
}


/**********************************************************************
*
* change direction
*
**********************************************************************
*/
void DK_change(UInt16 speed)
{
	sptr->SetPoint=speed;
}


/**********************************************************************
*
* change direction
*
**********************************************************************
*/
void DK_move(UInt16 speed)
{
	UInt8 count,i,tmp1,tmp2;
	
	count=0;
	IncPIDInit();
	sptr->SetPoint   = 670;
	sptr->LowRange = 600;
	sptr->HighRange = 850;
	CountEn=ENABLE;
	Count=0;
	sCSptr->B._startup = ENABLE;
	
	while(Count<=11);
	
	sCSptr->B._startup = DISABLE;
	P_TMR4_OutputCtrl->W = CW_TMR4_POLP_Active_High	|	\
						CW_TMR4_UPWM_Out_HL | CW_TMR4_UOC_Mode1 |\
						CW_TMR4_VPWM_Out_HL  | CW_TMR4_VOC_Mode1 |\
						CW_TMR4_WPWM_Out_HL  | CW_TMR4_WOC_Mode1;
	F_Delay(2);
	P_TMR4_OutputCtrl->W = CW_TMR4_POLP_Active_High	|	\
						CW_TMR4_UPWM_Out_HL | CW_TMR4_UOC_Mode0 |\
						CW_TMR4_VPWM_Out_HL  | CW_TMR4_VOC_Mode0 |\
						CW_TMR4_WPWM_Out_HL  | CW_TMR4_WOC_Mode0;

	CountEn=DISABLE;
	/* BLDC_Test4_PU120FullPWM(3);
	 P_TMR4_TGRA->W = MID2PWM;
	 P_TMR4_TGRD->W = MID2PWM  >>1;
	 P_TMR_LDOK->W = CW_TMR_LDOK1;
	 while((HALL>>5)!=2);
	 P_TMR4_TGRA->W = MINPWM;
	 P_TMR4_TGRD->W = MINPWM  >>1;
	 P_TMR_LDOK->W = CW_TMR_LDOK1;
	 
	P_TMR4_OutputCtrl->W = CW_TMR4_POLP_Active_High	|	\
	CW_TMR4_UPWM_Out_HL | CW_TMR4_UOC_Mode0 |\
	CW_TMR4_VPWM_Out_HL  | CW_TMR4_VOC_Mode0 |\
	CW_TMR4_WPWM_Out_HL  | CW_TMR4_WOC_Mode0;*/
}



/*****************************************************************************/
/* TPM4_ISR(): TPM4 period interrupt service routine						 */
/*****************************************************************************/
void TPM4_ISR(UInt16 hall)
{		
		if(sCSptr->B._startup == ENABLE)
			BLDC_Test4_NU120FullPWM(hall>>5);
}



/*=================================================================*/
/*BLDC 启动                          */
/*=================================================================*/
void BLDC_Motor_Startup(void)
{
	P_TMR0_Status->B.TCVIF = 1;
	if(sCSptr->B._startup==ENABLE)
	{		
		if(sCSptr->B._direction)
			BLDC_Test4_NU120FullPWM(P_POS1_DectData->W);
		else
			BLDC_Test4_PU120FullPWM(P_POS1_DectData->W);
	}
}

/*=================================================================*/
/*BLDC 正常运转                          */
/*=================================================================*/
void BLDC_Motor_Normalrun(UInt8 port)
{	UInt8 i;

//	P_TMR0_Status->B.PDCIF = 1;
	if(sCSptr->B._startup==ENABLE)
	{
		Speedcalc();	
		if(port == 3)
		{
			/*if(sCSptr->B._direction)
				BLDC_Test3_NU120FullPWM(P_POS0_DectData->W);
			else
				BLDC_Test3_PU120FullPWM(P_POS0_DectData->W);*/
		}
		else if(port == 4)
		{
			/*if(sCSptr->B._direction)
				BLDC_Test4_NU120FullPWM(P_POS1_DectData->W);
			else
				BLDC_Test4_PU120FullPWM(P_POS1_DectData->W);*/
		}
	}
}
/*=================================================================*/
/*BLDC 调速                          */
/*=================================================================*/
void BLDC_Motor_Actiyator(void)
{
	unsigned int uiSpeed = 0;

	if(sCSptr->B._startup)
	{
		//uiSpeed = 1875000L/(summation>>SHIFTDIV);		//计算速度

		if(iav<sptr->LowRange)
		{
			sptr->IncPoint += IncPIDCalc(iav);


		}
		else
		{
			if(iav>sptr->HighRange)
			{
				sptr->IncPoint += DecPIDCalc(iav);
			}

		}
			if(sptr->IncPoint > MAXPWM)	
				sptr->IncPoint = MAXPWM;
			if(sptr->IncPoint < MINPWM)	
				sptr->IncPoint = MINPWM;
			P_TMR4_TGRA->W = sptr->IncPoint;
			P_TMR4_TGRD->W = sptr->IncPoint  >>1;
			P_TMR_LDOK->W = CW_TMR_LDOK1;
		

	}
}
/*=================================================================*/
/*																   */
/*=================================================================*/
void Speedcalc(void)
{
	unsigned int original;

	original = P_TMR1_TGRA->W;
	//original=800;
	if(original > 469)			//最高转速限制在4000rpm（RADIX/4000）
	{
		summation -= *ptr;		//滑动滤波
		*ptr = original;
		summation += *ptr;
		if((++ptr) > (uiFilter+CAPBSIZE-1))		ptr = uiFilter;
	}
}


void IncPIDInit(void)
{	UInt8 i;
	
	sptr->IncPoint  = MID2PWM;
    	sptr->LastError = 0;
    	sptr->PrevError = 0;
	
	sptr->ProportionDec = KPP;
	sptr->ProportionInc= KPP/2;
    	sptr->Integral   = KPI;
    	sptr->Derivative = KPD;
    	sptr->SetPoint   = SETSPD;
	sptr->LowRange = SETSPD-100;
	sptr->HighRange = SETSPD+100;

	for(i=0;i<CAPBSIZE;i++)	uiFilter[i] = 0;
	ptr= uiFilter;
	summation = 0;
	sCSptr->B._startup = DISABLE;
}

/*=================================================================*/
/*   Increment PID计算                          */
/*=================================================================*/
static int IncPIDCalc(unsigned int NextPoint)
{
	register int dError, incpid;

	dError = sptr->SetPoint - NextPoint;

	incpid = sptr->ProportionInc* dError;
           //- sptr->Integral   * sptr->LastError;
          // + sptr->Derivative * sptr->PrevError;

	sptr->PrevError = sptr->LastError;
	sptr->LastError = dError;
	
	return(incpid);
}

UInt8 ReadHall()
{ 	UInt8  hall;

	hall = ((P_IOC_Data->W)&0x00e0)>>5;
	return (hall);
}

UInt8 ReadHallState()
{  UInt8 tmp1,tmp2,tmp3;
    UInt8 i,j;
	
	j=5;
	tmp1=ReadHall();

	tmp2=ReadHall();

	
	if( tmp1 != tmp2)
		return (0x01);
	else
		return (0xff);
	
}

static int DecPIDCalc(unsigned int NextPoint)
{
	register int dError, incpid;

	dError = sptr->SetPoint - NextPoint;

	incpid = sptr->ProportionDec* dError;
	sptr->LastError = dError;
	
	return(incpid);
}


/*=================================================================*/
/*	*END*                          */
/*=================================================================*/