9s12

/*PID算法模塊*/

typedef struct
{
	double ProportionalGain;
	double IntegralGain;
	double DerivativeGain;
	double InputErrorK_1;
	double IntegralPortionK_1;
	double PositivePIDLimit;
	double NegativePIDLimit;
}

PIDSING;
float DoublePIDLCalc(double GivenValue, double MeasureValue, PIDSING*pp)
{
	float dError, Error;
	Error = GivenValue - MeasureValue;											//計算Error
	pp -> IntegralPortionK_1 = Error + pp->IntegralPortionK_1;				//計算SUM error
	if(pp->IntegralPortionK_1 > pp->PositivePIDLimit){
		pp->IntegralPortionK_1 = pp->PositivePIDLimit;
	}
	if(pp -> IntegralPortionK_1 < pp->NegativePIDLimit){
		pp -> IntegralPortionK_1 = pp->NegativePIDLimit;
	}
	dError = dError - pp->IntegralPortionK_1;									//求誤差的微分
	pp -> InputErrorK_1 = Error;
	Error = pp->ProportionalGain*Error + pp -> IntegralGain*pp->IntegralPortionK_1 + IntegralGain*pp->IntegralPortionK_1;
																				//PID 公式計算
	if(Error > pp->PositivePIDLimit){
		Error = pp->PositivePIDLimit;
	}
	if(Error < pp->NegativePIDLimit){
		Error = pp->NegativePIDLimit;
	}
}

/*PWM功能模塊*/

#define MC33887__EN PTP_PTP7
#define MC33887__IN1 PTP_PTP3
#define MC33887__IN2 PTP_PTP5
/*Forward_Init()t*/
//假定前進方向，初始化函數
void Forward_Init (void)
{
	MC33887__EN =0;
	PWM_PWME3 	=0;
	PWM_PWME5	=0;
	PWMPRCLK | 		=0x40;	//CLOCK B 總線頻率的4分頻
							//通道3用clock B 時鐘源， 24MHZ/4 =6MHZ
	PWMPOL_PPOL3	=0;		//先低電平，對齊方式默認左對齊
	PWMCTL_CON23 	=1;		//16BIT鏈接
	PWMPER23	=750;		//電動機頻率8KHZ 1/1.5M*x=1/2KHz
	PWMDTY23	=300;		//占空比精度1/750
	PWMCNT23    =0;			//啟動PWM
	MC33887__EN	=1;			//MC33887使能
	MC33887__IN2=1;			//MC33887 IN 2 前進方向
	PWM_PWME3	=1;			//PWM通道輸出
} 
/*Backward_Init()*/
//假定後退方向。初始化函數
void Backwarf_Init (void)
{
	//Forward_Flag =0;
	MC33887__EN	=0;
	PWME_PWME5	=0;
	PWME_PWME3	=0;
	pwmprclk | 	=0X03;		//CLOCK A 總線頻率的4分頻
							//通道3用clock B 時鐘源， 24MHZ/4 =6MHZ
	PWMPOL_PPOL5=0;			//先低電平，對齊方式默認左對齊
	PWMCTL_CON45=1;			//16BIT鏈接
	PWMPER45	=750;		//電動機頻率8KHZ 1/1.5M*x=1/2KHz		
	PWMDTY45	=200;		//占空比精度1/750
	PWMCNT45 	=0;
	MC33887__EN	=1;			//MC33887使能
	MC33887__IN2=1;			//MC33887 IN 2 前進方向
	PWM_PWME5	=1;			//PWM通道輸出
}

void SetDuty (unsigned int temp)
{
	PWMDTY23	=temp;
}

/*速度測量子程序*/

/*ECT_Init()*/
//ECT初始化，使用輸入撲捉功能
void ECT_Init (void)
{
	TSCR1_TFFCA	=1;			//快速消除標誌
	TSCR2		=0x07;		//時鐘128分頻
	TIOS_IOS1	=0;			//通道1為輸入撲捉
	TCTL4_EDG1A	=1;			//撲捉通道1上升沿
	TCTL4_EDG1B	=0;			
	TIE_C1I		=1;			//通道1信號使能
	TSCR1_TEN	=1;			//定時器使能
}
ulong SpeedTemp[3]	={0, 0, 0};
#pragma CODE_SEG NON_BANKED
/*定是輸入撲捉中斷方式*/
#pragma TRAP_PROC
void Int_TimeCapture_C1(void)
{
	SpeedTemp[1]	=TC1;
	if(SpeedTemp[1] >= SpeedTemp[0]){
		SpeedTemp[2]=SpeedTemp[1]-SpeedTemp[0];
	}
	else{
		SpeedTemp[2]	=65536 - SpeedTemp[0]
		SpeedTemp[2]	+=SpeedTemp[1];
		/*SpeeTemp[2]為撲捉到的數，16位自由計數器的時鐘是187.5KHZ*/
	}
	SpeedTemp[0]	=SpeedTemp[1];
}
#pragma CODE_SEG DEFAULT

/*AD採樣函數*/
void AdInitial()
{
	ATD0CTL2	=0xc0;
	ATD0CTL3	=0x28;
	ATD0CTL4	=0x45;
}
unsigned int GetSampleResult (unsigned char channel)
{
	unsigned int sum, Max, Min, a[5];
	unsigned char i;
	ATD0CTL5	=0xa0 + channel;		//左對齊，無符號數，單次轉化
	while(!ATD0STAT0_SCF);				//等待一個轉換隊列完成
	a[0]	=ATD0DR0;
	a[1]	=ATD0DR1;
	a[2]	=ATD0DR2;
	a[3]	=ATD0DR3;
	a[4]	=ATD0DR4;
	Max		=a[0];
	Min 	=a[0];
	sum 	=a[0];
	for (i=1; i<5; i++){
		sum +=a[i];
		if(a[i] > Max){
			Max 	=a[i];
		}
		if(a[i] < Min){
			Min 	=a[i];
		}
	}
	return ((sum - Max - Min) / 3);
}

/*控制主程序*/
void RTI_Init (void)
{
	RtICTL 	=0x1f;
	CRGINT 	=0x80;
}
#pragma CODE_SEG NON_BANKED
#pragma TRAP_PROC
PIDSING pid;
void Int_RTI (void)
{
	unsigned int givenvalue;
	float pidvalue;
	CRGFLG 	=0x80;
	givenvalue 	=GetSampleResult(0);
	if(givenvalue >= 512){
		Forward_Init();
		givenvalue -=512;
	}
	else{
		Backward_Init();
		givenvalue 	=512 - givenvalue;
	}
	pidvalue 	=DoublePIDLCalc (givenvalue, (18750 - SpeedTemp[2])*512/18750, &pidvalue);
	SetDuty( (unsigned int ) pidvalue*65535/512);
}
#pragma CODE_SEG DEFAULT
void main (void)
{
	EnableInterrupts;
	pid.ProportionalGain	=0.1;
	pid.IntegralGain 		=0.08;
	pid.NegativePIDLimit	=0;
	pid.PositivePIDLimit	=65535;
	AdInitial();
	ECT_Init();
	RTI_Init();
	for(;;)
	{

	}
}