PID điều khiển động cơ

[boyngo_58]

Mình muốn điều khiển động cơ dùng PID,sử dung PIC 16F887, tần số thạch anh 4MHz, mình làm thế này:
- Trong chương trình ngắt timer1 để lấy mẫu 20ms
+ Đọc tốc độ từ encoder về
+ Tính sai số và tín hiệu điều khiển u
+ Điều chế xung PWM( chu kì 10Khz) cấp cho động cơ theo tín hiệu điều khiển u

Nhưng khi có những lệnh tính sai số e, u điều khiển thì tốc độ đọc về lại bị sai hết.
Không biết mình điều xung trong hàm ngắt 20ms thì có ảnh hưởng gì không?Mong mọi người giúp đỡ.....;)

 

[IceSandwich]

Điều xung phần cứng là độc lập với chương trình chính nên không lo ảnh hưởng, có thể quá trình xử lí số liệu của bạn có trục trặc gì đó, bạn có thể up code hay giải thuật để mọi người giúp đỡ.

 

[boyngo_58]

Mọi người xem giùm mình, mình viết code hơi khó hiểu mong mọi người thông cảm

void PID_init()
{
	T=0.02;
	Kp=0.1;
	Ki=0.08;
	Kd=1;
	A1=Kp + (Ki*T)/2 + Kd/T;
	A2=-Kp + (Ki*T)/2 -(2*Kd)/T;
	A3=Kd/T;
	e_1=0;
	e_2=0;
	u_1=0;
}

void dk_PID()
{
	//giai thuat PID
	e=setv-spd;                                   // tinh sai so xac lap
	u=u_1 + A1*e + A2*e_1 + A3*e_2;   // tin hieu dieu khien 
                                                     //u(k)=u(k-1)+A1*e(k)+A2*e(k-1)+A3*e(k-2)
	u_1=u;                                         // cap nhat  u(k-1), e(k-1), e(k-2)              
	e_2=e_1;
	e_1=e;
	// dieu xung
	int t1;
	t1=(u*400)/68;    // umax =68, PR2=99

	if(ktra=='F')  // Neu chieu quay la quay thuan, cap xung cho chan CCP1
	{
		char t2;
		// tat cap xung chan CCP2
		CCP2CON=0x0C;
		CCPR2L=0x00;

		CCPR1L=t1>>2;   // 8 bit cao
		t1=t1<<4;
		t2=t1&0x30;     //lay 2 bit 5 va 4
		CCP1CON=t2|0x0C;
		RB6=0;                  //den bao
	}
	else if(ktra=='P')
	{
		char t2;
		// tat cap xung chan CCP2
		CCP1CON=0x0C;
		CCPR1L=0x00;
		CCPR2L=t1>>2;
		t1=t1<<4;
		t2=t1&0x30;
		CCP2CON=t2|0x0C;
		RB6=0;
	}	

}	

//Ham phuc vu ngat

void interrupt isr(void)
{	
	// Khi Encoder có canh len
	if(INTF)			
	{
		INTF=0;				//Clear Flag
		if(clk==0)
		{
			TMR1ON=1;		//Enable timer1
		}
		clk++;				// Increase clk: so xung clock
	}
	// Timer1 20ms
	if(TMR1IF)
	{
		TMR1IF=0;			//Clear Flag;
		TMR1H=0xB1;			//Nap lai gia tri thanh ghi TMR1	
		TMR1L=0xDF;
		if(dem==0)
		{
			uart_putc(spd);  // gui gia tri 0 len PC de ve do thi
		}
		dem++;				//tang bien dem 
		RB7=0;
		spd=(clk/4);		//toc do dong co, dong co 200xung/vong, t=20ms
		if(dem<200)			//gui len PC 200 mau
		{
			uart_putc(spd);
			clk=0;                 // reset encoder
		}
		else
		{
			clk=0;              //reset encoder
		}

	if((ktra=='F')|(ktra=='P'))   // kiem tra khi dong co dc khoi dong thi dieu xung, 
                                             //F= chieu thuan, P chieu nguoc lai
		{
			dk_PID();
		}

	}
	
}