em viết đoặn mã cho PID như thế này, bác góp ý cho em có được ko nhé, em dùng công thức sai phân lùi để chuyển sang pt số
Code:
/Khai bao cac bien toan cuc cho xu lý PID
float u[2]; //u[1] là gia tri dau ra bo PID thoi diem hien tai, dc dua vao gia tri PDC1 de thay doi gia tri % duty cycle
//u[0] là gia tri dau ra bo PID thoi diem truoc do
int e[3]; //e[2] gia tri sai lech toc do tai thoi diem dang xet ( e[2] = toc do dat - toc do do dc tu encoder)
float A0,A1,A2;
float PID_P = 7, PID_I =1.3, PID_D =2.7; //cac he so Kp, Ki, Kd
//Chuong trinh khoi tao thong so PID
//------------------------------------------------------------------------------
void PID_init(void)
{
u[0] =0;
u[1] =0;
e[0] =0;
e[1] =0;
e[2] =0;
A0 = PID_P + PID_I + PID_D;
A1 = -PID_P - 2*PID_D;
A2 = PID_D;
}
//Chuong trinh xu ly ngat Timer 2 sau 10ms
//chu ki trich mau 10ms de tinh toan PID
//------------------------------------------------------------------------------
void __attribute__((__interrupt__)) _T2Interrupt(void)
{
e[2] = (ADCBUF0 >> 1) - (POSCNT*3 ); //e[2] gia tri sai lech toc do tai thoi diem dang xet
//e[2] = toc do dat - toc do do dc tu encoder
// toc do dat = ADCBUF0 >>1 (giá tri chuyen doi ADC bien tro dieu chinh toc do, lay 9 bit)
// toc do do dc tu encoder = POSCNT * 3
POSCNT = 0;
//u[n] = u[n-1] + A0*e[n] +A1*e[n-1] +A2*e[n-2]
u[1] = u[0] + e[2]*A0 + e[1]*A1 + e[0]*A2;
if (u[1] > 511) //Gioi han tren
{ u[1] = 511;}
if (u[1]<0) //Gioi han duoi
{ u[1] = 0;}
u[0] = u[1];
e[0] = e[1];
e[1] = e[2];
PDC1 = u[1];
}