## 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

{
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;
ECT_Init();
RTI_Init();
for(;;)
{

}
}``````

### 電泡泡

#### 暂无文章

4分钟前
0
0
Double数相加后结果不准确

12分钟前
0
0

originDu
13分钟前
0
0

18分钟前
1
0
Kotlin的SAM（Single Abstract Method）

SuShine
20分钟前
1
0