PIC Vietnam - Lập trình CCS cho PIC16F887 đo và điều xung động cơ

Em làm đồ án có nội dung là đo và điều khiển tốc độ động cơ DC sử dụng PIC6F887
Em lập trình bằng CCS, mô phỏng proteus.
Dưới đây là code em xin cac đàn anh xem qua để chỉnh sửa giúp, , vì mạch của em vẫn chưa chạy đúng như ý muốn.

Sơ lược qua hoạt động em thiết kế:
-Mạch dùng 1 pic, nhận xung từ motor-encoder và đếm số xung trong 500ms, rồi từ đó tính ra số vòng quay đo được trong 1 phút hiển thị lên LCD liên tục
-3 nút nhấn ở chân RB5,6,7 dùng để nhập số vòng quay mong muốn vào (3 nút là giá trị của hàng trăm, chục, đvi). Xuất giá trị này lên LCD liên tục.
-PIC sẽ sử dụng chân pwm để tăng giảm duty tùy thuộc vào so sánh "số xung đếm dc trong 500ms" và "số xung cần có trong 500ms" (tính từ số vòng quay nhập vào).

Em chỉ ko rành lắm về khai báo ngắt ngoài (đếm xung) và pwm (điều xung) của mình đã đúng hay chưa, mong mọi người chỉ giúp.

Đây là code của em:

#include <16F887.h>
#fuses HS,NOWDT,NOPROTECT,NOLVP
#use delay(clock=20000000)

#use rs232(baud=9600, xmit=PIN_C6, rcv=PIN_C7)
#include "dpLCD420.c"
#define INTS_PER_SECOND 10 //dem xung trong 500ms (1s = 20000000/4/4/65536)
int16 a=0; //RPM nhap vao
int8 a1,a2,a3; // a=a1+10a2+100a3
int8 b=100; //So xung 1 vong
int16 duty=500,t; //PWM
int16 count=0; //So xung dem dc trong 0.5s
int8 int_count; // Number of interrupts left before a second has elapsed

#INT_EXT
void isr_EXT()
{ count++;}

#INT_TIMER1
// Ham nay goi moi khi timer1 tran so (65535 ->0), la khoang 19 lan/s hoac 10lan/0.5s
void clock_isr() {

if(--int_count==0) {
printf("Count= %Lu \n\r",count);

//PWM
//chia ra cac khoang (t-100) (t-50) (t-20) t (t+20) (t+50) (t+100)
t=a*b/60/2; //so xung can co trong 0.5s tinh theo so a=RPM nhap vao
if ( count<t-100 ) { duty=duty+30; }
if ( (t-100<count) & (count<t-50) ) { duty=duty+20; }
if ( (t-50 <count) & (count<t-20) ) { duty=duty+10; }
if ( (t-20 <count) & (count<t) ) { duty=duty+5 ; }
if ( count=t ) { duty=duty ; }
if ( (t<count) & (count<t+20 ) ) { duty=duty-5 ; }
if ( (t+20<count) & (count<t+50 ) ) { duty=duty-10; }
if ( (t+50<count) & (count<t+100) ) { duty=duty-20; }
if ( t+100<count ) { duty=duty-30; }

if(duty>1000) { duty=1023; output_high(PIN_B1);} //Led bao khi motor dat van toc max
if(duty<100) { duty=100; output_high(PIN_B1);} //Led bao khi motor dat van toc min

count=0;
int_count = INTS_PER_SECOND;
set_pwm1_duty(duty);
}
}

void lcd()
//Dung kem theo file dpLCD
{
while(!input(PIN_B7)) //RB7 chon hang don vi a1
{
delay_ms(100);
a1++; if (a1==10) {a1=0;}
a=100*a3+10*a2+a1;
lcd_putc("\f");
lcd_gotoxy(1,1);
printf(lcd_putc,"RPM %Lu ",60*2*count/b); //Xuat RPM do duoc
lcd_gotoxy(1,2);
printf(lcd_putc,"Ref RPM %Lu ",a); //Xuat RPM mong muon
delay_ms(300);
}

while(!input(PIN_B6)) //RB6 chon hang chuc a2
{
delay_ms(100);
a2++; if (a2==10) {a2=0;}
a=100*a3+10*a2+a1;
lcd_putc("\f");
lcd_gotoxy(1,1);
printf(lcd_putc,"RPM %Lu ",60*2*count/b); //Xuat RPM do duoc
lcd_gotoxy(1,2);
printf(lcd_putc,"Ref RPM %Lu ",a); //Xuat RPM mong muon
delay_ms(300);
}

while(!input(PIN_B5)) //RB5 chon hang tram a3
{
delay_ms(100);
a3++; if (a3==10) {a3=0;}
a=100*a3+10*a2+a1;
lcd_putc("\f");
lcd_gotoxy(1,1);
printf(lcd_putc,"RPM %Lu ",60*2*count/b); //Xuat RPM do duoc
lcd_gotoxy(1,2);
printf(lcd_putc,"Ref RPM %Lu ",a); //Xuat RPM mong muon
delay_ms(300);
}
}

void main()
{
//khai bao doc xung
enable_interrupts(INT_TIMER1);
enable_interrupts(INT_EXT);
enable_interrupts(GLOBAL);
int_count = INTS_PER_SECOND;
setup_timer_1(T1_INTERNAL | T1_DIV_BY_4); //43.6 ms
//set_timer1(0);

//khai bao lcd
lcd_init();
lcd_putc("\f");
lcd_putc("RPM do duoc \n");
lcd_putc("RPM mong muon");
delay_ms(1000);
//Ban dau cho mac dinh Motor quay voi van toc a = 555 rpm
a1=5;
a2=5;
a3=5;

//khai bao pwm
setup_ccp1(CCP_PWM); // Configure CCP1 as a PWM
setup_timer_2(T2_DIV_BY_4,255,1);
set_pwm1_duty(duty);
while(TRUE)
{
lcd();

}
}

Em gửi kèm mạch mô phỏng proteus cho rõ ràng hơn
Em cám ơn và chờ góp ý :D