cần sự giúp đỡ về PID điều khiển tốc độ động cơ dùng winform và pic16f877a

[Trắng bạch]

em đang làm cái đồ án sử dụng pid để điều khiển Động Cơ DC Servo Giảm Tốc GA25 12v/280rpm, dùng winform truyền xuống pic16f877a các thông số vận tốc, ki, kd, kp. hiện tại em đã truyền xuống được nhưng truyền xuống vài lần là giao diện winform bị đơ, em cũng đã dò rất nhiều lần các thông số ki kd kp nhưng vận tốc thực tế vẫn luôn giao động rất lớn. Mọi người ai biết gì chỉ giúp em với ạ, dưới đây là code của pic
#include <main.h>
#include <stdio.h>
#include <stdbool.h>
#include <stdlib.h>
#define LCD_ENABLE_PIN PIN_C4
#define LCD_RS_PIN PIN_D2
#define LCD_RW_PIN PIN_D3
#define LCD_DATA4 PIN_D4
#define LCD_DATA5 PIN_D5
#define LCD_DATA6 PIN_D6
#define LCD_DATA7 PIN_D7
#define IN1 PIN_B6
#define IN2 PIN_C2
#define led PIN_B3
//them thu vien lcd
#include <lcd.c>
#include <string.h>

double xung=0.0;
double T=0.1;
double tocdodat=0, tocdo=0;
double E=0, E1=0, E2=0;
double a=0,b=0,c=0,kp=0,ki=0,kd=0;
double output=0, lastoutput=0;
char str[30]="";

#INT_EXT
void Interupt(void)
{
xung=xung+2;

}

#INT_RDA
void Ngat_Nhan(void)
{
gets(str);
delay_ms(50);
clear_interrupt(INT_RDA);
}
void main()
{
char *token;
float numbers[4];
char *s1="#";
output_high(led);
set_tris_b(0x01);
lcd_init(); //Khoi tao lcd
setup_timer_2(T2_DIV_BY_16, 255, 1);
setup_ccp2(CCP_PWM);
setup_timer_1(T1_INTERNAL|T1_DIV_BY_8 );
SET_TIMER1(53465);
enable_interrupts(INT_TIMER1);
ext_int_edge(H_TO_L); // Ch?n ng?t theo su?n âm
clear_interrupt(INT_EXT);
enable_interrupts(int_EXT);
clear_interrupt(INT_RDA);
enable_interrupts(INT_RDA);//cho phep ngat uart
enable_interrupts(GLOBAL);

while(true){


int i = 0;
token = strtok(str, s1);
while (token != NULL) {
numbers[i++] = atof(token);
token = strtok(NULL, s1);
}
tocdodat=numbers[0];
kp=numbers[1];ki=numbers[2];kd=numbers[3];
lcd_gotoxy(1,1);
printf(lcd_putc," Vdat = %0.0f ",tocdodat);
lcd_gotoxy(1,2);
printf(lcd_putc," V = %f ",tocdo);
printf("%f#%f#%f#%f \r\n",tocdo,kp,ki,kd);
}
}
#int_TIMER1
void TIMER1_isr(void)
{
enable_interrupts(INT_RDA);//cho phep ngat uart
output_toggle(led);
tocdo = (xung / 374) * (1 / T) * 60;
E = tocdodat - tocdo;
a = 2*T*Kp + Ki*T*T + 2*Kd;
b = T*T*Ki - 4*Kd - 2*T*Kp;
c = 2*Kd;
output = (a*E + b*E1 + c*E2 + 2*T*lastoutput)/(2*T);
lastoutput = output;
E2=E1;
E1=E;
if(output>255)
output =255;
if(output<0)
output=0;
set_pwm2_duty(output);
output_low(IN1);

xung=0;
SET_TIMER1(53465);
}