文档章节

9s12

電泡泡
 電泡泡
发布于 2014/03/07 17:45
字数 718
阅读 118
收藏 0
/*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(;;)
	{

	}
}



© 著作权归作者所有

共有 人打赏支持
電泡泡
粉丝 24
博文 183
码字总数 69717
作品 0
衡阳
私信 提问

暂无文章

二十分钟教你如何将区块链应用与函数计算相结合

前言 本篇文章适合对区块链应用感兴趣或是想要通过函数计算服务进一步开发区块链应用的新人。本文将结合阿里云区块链服务、阿里云函数计算服务、阿里云日志服务 以及社区应用 Marbles,手把手...

阿里云官方博客
4分钟前
0
0
Double数相加后结果不准确

在我们进行两个double运算时,例如:2..0-1.1 不是想象的输出0.9,而是0.89999999999999999。其主要原因是浮点数值采用二进制系统表示,而在二进制系统中无法精确的表示分数1/10。这就好像十...

嘴角轻扬30
12分钟前
0
0
去除移动端点击效果

移动端点击时,会有一个类似active的短暂背景淡出效果,去除该效果可使用 -webkit-tap-highlight-color: rgba(255, 0, 0, 0);

originDu
13分钟前
0
0
腾讯云与MariaDB 基金会签署战略合作,共建全球开源生态圈

本文由云+社区发表 腾讯云日前与MariaDB基金会正式签署战略合作协议,2019年,腾讯云将继续以白金会员身份为基金会的发展提供强有力的资源支持,与MariaDB全球用户和开发者一道,共建开放共赢...

腾讯云加社区
18分钟前
1
0
Kotlin的SAM(Single Abstract Method)

今天有人在群里问kotlin支持SAM的问题,其实kotlin不支持SAM,因为人家支持FP(function programing) package reactinterface Test { fun print()}class TestInterface(var...

SuShine
20分钟前
1
0

没有更多内容

加载失败,请刷新页面

加载更多

返回顶部
顶部