9s12-白红宇

9s12

阅读量：6112 次

发布时间：2019-06-21

本文共 3919 字，大约阅读时间需要 13 分钟。

/*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_PROCvoid 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_PROCPIDSING 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 DEFAULTvoid main (void){	EnableInterrupts;	pid.ProportionalGain	=0.1;	pid.IntegralGain 		=0.08;	pid.NegativePIDLimit	=0;	pid.PositivePIDLimit	=65535;	AdInitial();	ECT_Init();	RTI_Init();	for(;;)	{	}}