PIC Vietnam - Giúp đỡ về cảm biến siêu âm với module tự làm

Chào mọi người!
Hiện nay mình đang làm đồ án về đo khoảng cách bằng sóng siêu âm. Mình ko mua module SRF05 mà mình tự làm. Mình tìm sơ đồ mạch trên mạng như file đính kèm. Mình có thêm vào là bên thu có bộ khuếch đại 220 lần và so sánh điện áp dùng opamp LM324.
Mình dùng pic 16f877a.
Mình lấy chân phát từ chân RE2, chân thu là RB0.
Nguyên lý của mình là đo thời gian phát và thu, s=(v*t)/2 với v=340m/s.
Trước khi phát mình bật timer1, phát 8 xung, tần số 40Khz, mỗi 8 xung cách nhau 3ms.
Khi có tín hiệu vào chân RB0 ( ngắt ngoài) thì dùng lệnh get_timer1 để lấy thời gian, đồng thời tắt ngắt. Tính khoảng cách s và hiển thị lên LCD 4 bit.
Module siêu âm của mình thì chắc ko hư vì đứa bạn cũng làm nguyên lý này nhưng viết bên 8051 thì chạy ok. Mình nghĩ do code, Mọi người giúp mình với.
Code:

#include <16F877A.h>
#include <def_877a.h>
#use delay(clock=12000000)
#include <LCD_LIB.c>
#FUSES NOWDT, HS, NOPUT, NOPROTECT, NODEBUG, NOBROWNOUT, NOLVP, NOCPD, NOWRT

int8 j=0;
int1 sta=0;
int16 time,v=340;
float s=0,s1=0;

#int_timer1
void TIMER1_isr()
{
set_timer1(63286);
for(j=0;j<8;j++)
{
RE2=1;
delay_us(11);
RE2=0;
delay_us(11);
}
}

#int_ext
void ngat_RB()
{
time=get_timer1();
disable_interrupts(int_ext);
disable_interrupts(int_timer1);
disable_interrupts(global);
s1 = (v * time)/2;
s=s1/1000;
sta=1;
}

void main()
{
set_tris_b(0xff);
set_tris_d(0x00);
set_tris_e(0x00);
enable_interrupts(int_timer1);
setup_timer_1(T1_INTERNAL|T1_DIV_BY_4);
enable_interrupts(global);

enable_interrupts(int_ext);
ext_int_edge(H_TO_L);
LCD_init();
delay_ms(100);
Printf(LCD_putc,"DIST = %5.2f cm", s);
delay_ms(100);
set_timer1(63286);//time= 4*(65536-63286)*1/3us=3000us=3ms
while (TRUE)
{
if(sta==1)
{
sta=0;
lcd_gotoxy(7,1);
printf(lcd_putc," ");
delay_ms(10);
lcd_gotoxy(7,1);
printf(lcd_putc,"%5.2f cm", s);
delay_ms(10);
enable_interrupts(global);
enable_interrupts(int_ext);
enable_interrupts(int_timer1);
set_timer1(63286);

}
}
}