Nguyên lí chạy tốt,mạch thật chạy sai,ai giúp mình cái điều khiển động cơ này với

[boy_legend]

Mình làm đồ án về điều khiển động cơ DC có encoder hồi tiếp về.Mục đích của đồ án này là cấp cho vđk 1 số lượng xung nào đó, thì vdk sẽ điều khiển cho động cơ chạy đúng theo số lượng xung như thế, nếu chạy quá nhanh , vđk sẽ tự hãm lại, ngược lại sẽ cho động cơ quay nhanh hơn.Code và nguyên lí mình đã hoàn thành xong, nhưng khi ra mạch thật thì chạy sai bét.LCD thì chẳng chịu hiển thị, input vào đo bằng timer 0 cũng chẳng chịu đo, phát xung ra bậy bạ, riêng chỉ có mạch cầu H thiết kế hoạt động tốt.Mình đang bế tắc không biết nguyên nhân vì sao cả, khi code không có warning nào cả, cũng như nguyên lí cũng không thấy có báo warning nào cả.Động cơ mình dùng nguồn 12V, 2A, encoder 100xung/vòng, input mình cũng dự định dùng encoder riêng ở ngoài 100xung/vòng, sử dụng mạch nạp Burn-E để nạp code.Xin mọi người giúp dùm mình với.
Sau đây là code và mạch của mình

 
#include <16f877a.h>
#include <DEFS_16f877A.h>
#fuses NOWDT,HS,PUT,NOPROTECT,NOLVP
#device *=16 ADC=8
#use delay(clock=8000000)
#include "LCD420.c"
#include <math.h>

signed int16 Pulse_IN,Pulse_ENC,Pulse_ENCn;
signed int16 sub_ENC=0,sub_IN=0,sub_IN2=0,sub_ENC2=0;
int1 flag=0;

void display(unsigned int16 data)
{ 
   lcd_putc(data/100+0x30);
   lcd_putc((data%100)/10+0x30);
   lcd_putc(data%10+0x30); 
}

void LCD()
{
      lcd_gotoxy(1,1);
      lcd_putc("IN: ");
      display(Pulse_IN);
      if(RA3==0)
      {
      lcd_gotoxy(9,1);
      if(Pulse_ENC>=0)
      {
      lcd_putc("ENC :");
      display(Pulse_ENC);      
      }
      else
      {
      lcd_putc("ENC:");
      lcd_putc("-");
      display(-Pulse_ENC);
      }
      lcd_gotoxy(1,2);
      lcd_putc("IN>:");
      display(sub_IN);      
      lcd_gotoxy(9,2);
      lcd_putc("ENC>:");
      display(sub_ENC);
      }
      else
      {
      lcd_gotoxy(9,1);
      if(Pulse_ENCn>=0)
      {
      lcd_putc("ENC :");
      display(Pulse_ENCn);      
      }
      else
      {
      lcd_putc("ENC:");
      lcd_putc("-");
      display(-Pulse_ENCn);
      }
      lcd_gotoxy(1,2);
      lcd_putc("IN>:");
      display(sub_IN2);      
      lcd_gotoxy(9,2);
      lcd_putc("ENC>:");
      display(sub_ENC2);
      }
}

void PWM1(unsigned long speed)
{
   RE0=1;
   delay_us(speed);
   RE0=0;
}

void PWM2(unsigned long speed)
{
   RE1=1;
   delay_us(speed);
   RE1=0;
}

#int_ext
void demxung()
{
   if(RB1==0) 
   {
   Pulse_ENC--;
   Pulse_ENCn++;
   }
   else 
   {
   Pulse_ENC++;
   Pulse_ENCn--;
   }
}
void main()
{
   lcd_init();
   set_tris_a(0xff); //Port a input
   set_tris_b(0xff);//RB0-RB1-input
   set_tris_e(0x00); //Port e output
   PORTA=0x00;
   PORTE=0x00;
   PORTB=0x00;
   enable_interrupts(int_ext);
   ext_int_edge(H_TO_L);;
   setup_timer_0(RTCC_DIV_1|RTCC_EXT_H_TO_L);
   set_timer0(0);
   enable_interrupts(global);
   while(1)
   {
   Pulse_IN=get_timer0();
   if(RA3==0)
   {
   if(Pulse_IN==Pulse_ENC)
   {
   sub_ENC=0;
   sub_IN=0;   
   }
   else if(Pulse_IN>Pulse_ENC)
   {
   sub_IN=Pulse_IN-Pulse_ENC;
   }
   else 
   {
   sub_ENC=Pulse_ENC-Pulse_IN;
   }
   LCD();
   if(Pulse_IN>0&&sub_ENC==0)
   {
   PWM1(20000);
   //delay_us(40000);
   flag=1;
   }   
   if(sub_ENC>0)
   {
   if(sub_ENC>10)
   {
   PWM2(20000);
   sub_ENC--;
   }
   else if(sub_ENC>8)
   {
   PWM2(1200);
   sub_ENC--;
   }
   else if(sub_ENC>5)   
   {
   PWM2(1000);
   sub_ENC--;
   }
   else if(sub_ENC>4)
   {
   PWM2(800);
   sub_ENC--;
   }
     else if(sub_ENC>3)
   {
   PWM2(500);
   sub_ENC--;
   }
    else if(sub_ENC>2)
   {
   PWM2(300);
   sub_ENC--;
   }
   else
   {
   PWM2(100);
   sub_ENC--;
   }
   }
   if(sub_IN>0)
   {
   if(sub_IN>10)
   {
   PWM1(20000);
   sub_IN--;
   }
   else if(sub_IN>8)
   {
   PWM1(1200);
   sub_IN--;
   }
   else if(sub_IN>5)
   {
   PWM1(1000);
   sub_IN--;
   }
   else if(sub_IN>4)
   {
   PWM1(800);
   sub_IN--;
   }
   else if(sub_IN>3)
   {
   PWM1(500);
   sub_IN--;
   }
   else if(sub_IN>2)
   {
   PWM1(300);
   sub_IN--;
   }
   else
   {
   PWM1(100);
   sub_IN--;
   }
   }
   if(sub_IN==0&&sub_ENC==0&&flag==1)
   {   
   set_timer0(0);
   Pulse_ENC=0;
   Pulse_ENCn=0;
   flag=0;
   delay_ms(4);
   }
   }
   else                                            /////////////Quay nghich/////////
   {
   if(Pulse_IN==Pulse_ENCn)
   {
   sub_ENC2=0;
   sub_IN2=0;   
   }
   else if(Pulse_IN>Pulse_ENCn)
   {
   sub_IN2=Pulse_IN-Pulse_ENCn;
   }
   else 
   {
   sub_ENC2=Pulse_ENCn-Pulse_IN;
   }
   LCD();
   if(Pulse_IN>0&&sub_ENC2==0)
   {
   PWM2(20000);
   flag=1;
   }   
   if(sub_ENC2>0)
   {
   if(sub_ENC2>10)
   {
   PWM1(20000);
   sub_ENC2--;
   }
   else if(sub_ENC2>8)
   {
   PWM1(1200);
   sub_ENC2--;
   }
   else if(sub_ENC2>5)   
   {
   PWM1(1000);
   sub_ENC2--;
   }
   else if(sub_ENC2>4)
   {
   PWM1(800);
   sub_ENC2--;
   }
   else if(sub_ENC2>3)
   {
   PWM1(500);
   sub_ENC2--;
   }
   else if(sub_ENC2>2)
   {
   PWM1(300);
   sub_ENC2--;
   }
   else
   {
   PWM1(100);
   sub_ENC2--;
   }
   }
   if(sub_IN2>0)
   {
   if(sub_IN2>10)
   {
   PWM2(20000);
   sub_IN2--;
   }
   else if(sub_IN2>8)
   {
   PWM2(1200);
   sub_IN2--;
   }
   else if(sub_IN2>5)
   {
   PWM2(1000);
   sub_IN2--;
   }
   else if(sub_IN2>4)
   {
   PWM2(800);
   sub_IN2--;
   }
   else if(sub_IN2>3)
   {
   PWM2(500);
   sub_IN2--;
   }
   else if(sub_IN2>2)
   {
   PWM2(300);
   sub_IN2--;
   }
   else
   {
   PWM2(100);
   sub_IN2--;
   }
   }
   if(sub_IN2==0&&sub_ENC2==0&&flag==1)
   {   
   set_timer0(0);
   Pulse_ENC=0;
   Pulse_ENCn=0;
   flag=0;
   delay_ms(4);
   }
   }
}           
}

Giải thích phần code:
Code mình sẽ đếm số xung từ đầu vào bằng timer0, vì dự định dùng encoder tạo xung tay tốc độ 100xung/vòng nên 8bit đếm của timer0 là đủ.Nhận được xung lớn hơn giá trị 0 thì sẽ cho động cơ chạy.Encoder thứ 2 gắn trên động cơ sẽ hồi tiếp giá trị về, giá trị này là Pulse_ENC.Sau đó VDK sẽ so sánh 2 giá trị Pulse_IN và Pulse_ENC này để tìm ra sai số sub_IN(nếu IN > ENC) và sub_ENC(nếu ENC>IN), từ đó cho động cơ chạy thêm hoặc hãm lại để điều chỉnh cho 2 cái sub này tiến về 0 hay nói cách khác là để Pulse_ENC = Pulse_IN.Để quay mượt mình chia nhiều cấp độ, nếu sai số lớn thì sẽ cho hãm mạnh, chạy thêm mạnh, sai số nhỏ sẽ hãm yếu hơn, như thế động cơ sẽ không bị giật qua giật lại.