Đo tốc độ động cơ - PID - pic30

[kokonomutsu]

Hiện tại em có làm đồ án về điểu khiển tốc độ động cơ, (nếu có thể sẽ làm vị trí) và có vài thắc mắc nhờ các thầy và các anh giải đáp như sau:
- Đo vận tốc động cơ bằng encoder như thế nào là tối ưu, em đã sử dụng cách làm sau để đo tốc độ động cơ, nhưng khi set giá trị PDC=const thì tốc độ động cơ vẫn thay đổi (em cho hiển thị lên LCD và nhận thấy sai số khoảng +/-10:
/**************************************************/
void _ISR _T2Interrupt(void)
{
_T2IF = 0;
vout = (round_number + ((float)POSCNT)/MAXCNT)*60*50*2;
//tinh so vong quay duoc trong 1 phut, so sanh voi vset, nhan 50 khi lay mau 20ms
//viec cap nhat xay ra sau moi 20ms dinh boi timer 2
//van toc max khi o 16v la khoang 750 vong/phut. de tien cho viec quan sat nhan cho 2, toc do dat cho phep trong khoang 0-1500
round_number=0;
POSCNT=0;
velocity_pid();
}
/**************************************************/
Chương trình trên em làm với mục đích đo tốc độ vòng/phút của động cơ với thời gian nhảy vào ngắt là 20ms. trên thực tế khi em set PDC1=const thì tốc độ của em đo được vẫn dao động rất nhiều (em chưa gọi hàm PID).
Cách em đo tốc độ động cơ vậy đã tốt hay chưa? Hay em nên đo bằng cách set trước số xung bằng const ( ví dụ 1000) rồi đo thời gian động cơ quay được 1000 xung từ đó tính được tốc độ đông cơ.
Đây là code của hàm main của em:
- #include <p30F6015.h>
_FOSC(CSW_FSCM_OFF & HS);
_FWDT(WDT_OFF);
_FBORPOR(PBOR_OFF & MCLR_EN & PWMxH_ACT_HI );
_FGS(CODE_PROT_OFF); //code protect
int count, count_tmr1;
int error2, error1, error_sum, error_del, round_number;
unsigned int control, vout, vin, Kp,Ki,Kd;
void _ISR _T1Interrupt(void);
//Chuong trinh chinh
int main(void)
{
XlcdInit();
lcd_clear();
lcd_puts("Vin:");
init_pwm(999); //Khoi tao pwm voi thach anh 20MHz, bam 5khz mode HS
init_const();
XlcdCursor(1,11);
lcd_display_number(vin,4); //Vong lap chinh
init_timer1(); //Khoi tao Timer 1
init_qei();
init_timer2();
while (1)
{
};
}
//Chuong trinh xu ly ngat Timer 1
void _ISR _T1Interrupt(void)
{
_T1IF = 0; //Xoa co ngat
XlcdCursor(2,11);
lcd_display_number(vout,4);
}
/**************************************************/
void _ISR _T2Interrupt(void)
{
_T2IF = 0;
vout = (round_number + ((float)POSCNT)/MAXCNT)*60*50;
//tinh so vong quay duoc trong 1 phut, so sanh voi vset, nhan 50 khi lay mau 20ms
//viec cap nhat xay ra sau moi 20ms dinh boi timer 2
//van toc max khi o 16v la khoang 750 vong/phut. de tien cho viec quan sat nhan cho 2, toc do dat cho phep trong khoang 0-1500
round_number=0;
POSCNT=0;
velocity_pid();
}
/**************************************************/
void _ISR _QEIInterrupt(void)
{
_QEIIF = 0;
round_number++;
}
/**************************************************/
void velocity_pid()
{
error2 = vin - vout;
#define MAX_CONTROL 59940//=gia tri max PDC * 30
control+=__builtin_mulsu(error2,Kp)>>7;
if(control > MAX_CONTROL)
{
control = MAX_CONTROL;
}
else
{
control+=__builtin_mulsu((error2-error1),Kd)>>7;
error1=error2;
error_sum+=error2;
control+=__builtin_mulsu(error_sum,Ki)>>7;
}
#define scale_pwm 30//gia tri scale nay tinh tu max cua PDC1=1998=2*999 ~ tan so pwm)
PDC1=__builtin_divsd(control,scale_pwm);
}
/************************************/
void init_const()
{
vout = 0;
round_number = 0;
vin=425;
Kp=540;//500
Ki=10;//4
Kd=1;//1
error2=0;
error1=0;
error_sum=0;
error_del=0;
control=0;
// PDC1 = 1850;
}
/************************************************** *****************************/
Chú thích:
Với hàm init_pwm(999) khởi tạo pwm tần số 5KHz.
init_const() khởi tạo thông số ban đầu.
init_timer1() timer 1 hiển thị tốc độ động cơ sau 1s.
init_qei() khởi tạo module qei.
Các module em đã test và hoạt động ổn định.
Mong nhận được giúp đỡ của các thấy và các anh em, nếu có tài liệu liên quan cho em xin, em xin cám ơn rất nhiều.