PIC Vietnam - cửa tự động đầu vào là cảm biến siêu âm và các công tác hành trình

xin chào các anh chị trong diễn đàn!
em là người mới làm việc với vi điều khiển pic,hiện nay em đang làm đề tài về cửa đóng mở tự động sơ qua như sau :
khi khoảng cách đo được từ cảm biến siêu âm đạt đến giá trị đạt thì cửa sẽ mở và khi người ra khỏi cửa thì của đóng vào,việc đóng mở cửa em dụng động cơ cộng với l298 để đảo chiều, việc tăng giảm tốc độ thì dùn pwm từ chân 16 của vdk.lúc đầu em căm test trên board thực tế thì chay tốt thế nhưng khi ghép chúng lại với nhau thì không được hôm nay bế tắc quá nên quyết đinh viết lên hỏi các anh chị trên diễn đàn.rất mong được sụ giúp đỡ. đây là code của chương trình (code này em có tham khảo trên picvietnam và phát triển thêm)

#include <16f877a.h>
#device *=16 ADC = 8
#fuses HS,NOPUT,NOWRT,NOLVP,NOCPD,NOWRT,NODEBUG
#use delay(clock=20000000)
#include <lcd.c>
#byte portb=0x06
#bit B1=0x06.1
#byte trisb=0x86
#bit wpub0=0x95.0
#bit wpub1=0x95.1
#bit wpub2=0x95.2
#bit wpub3=0x95.3
#bit wpub4=0x95.4
#bit wpub5=0x95.5
#bit wpub6=0x95.6
#bit wpub7=0x95.7
#byte porta=0x05
#byte portc=0x07
#byte trisa=0x85
#byte TRISC=0X87
#bit trisc6=0x87.6
#bit trisc7=0x87.7
#bit TRISC2=0X87.2
#bit TRISC1=0X87.1
#BIT RC2 = 0x7.2
#bit c6 = 0x07.6
#bit c7 = 0x07.7
char a[4]={0x0E,0X0D,0X0B,0X07};
char kitu[10];
float getvar = 0, distance = 0;
int1 has_echo = 0, trg = 0, button = 0,button1=0;
void init(); //Thi?t l?p lcd, timer1, Capture
void trigger(); //tao trigger
//void dongco();
void xuatxung(int value);
char ascii_code[4][4]={'7','8','9','#',
'4','5','6','*',
'1','2','3','-',
'N','O','=','+'};
unsigned int key;
void checkkey();
void checkkey()
{
int i,j,key,dem,h,t;
for (i=0;i<4;i++)
{
trisb=0xff-(1<<(i+4));
delay_us(10);
key=portb & 0x0f;
if (key != 0x0f)
{
for (j=0;j<4;j++)
if (key == a[j])
{
while ((portb & 0x0f) ==a[j]) ;
lcd_putc("\f");
dem++;
if(dem==16) dem=0;
kitu[dem]=ascii_code[j][i];
}
}
}
for (h=1;h<=dem;h++)
{
lcd_gotoxy(h,0);
lcd_putc('*');
}
if (dem==3)
{
if (kitu[1]=='6' &&kitu[2]=='7' && kitu[3]=='8')
{
dem=0;
lcd_putc("\f");
lcd_putc("\fSRF05 measure");
lcd_gotoxy(1,0);
lcd_putc("PICvietnam.com");
delay_ms(200);
button1=1;
}
else
{
lcd_gotoxy(0,0);
lcd_putc(" ban nhap sai");
dem=0;
}
}
}
#INT_CCP1
void ccp1_isr()
{
if(!trg) {
//Khi có xung echo canh lên
set_timer1(0); //Reset timer1 v? 0
setup_ccp1(CCP_CAPTURE_FE); //chuyen sang dò xung canh xuong
trg = 1;
} else {
//Khi có xung echo c?nh xu?ng
getvar = get_timer1(); //doc giá tri timer1 ngay khi có xung echo csnh xuong
distance = (getvar - 485)/28.3; // So lieu dua theo ket qua do thuc te
has_echo = 1;
disable_interrupts(GLOBAL);
}
}
void main()
{
init();
port_b_pullups(true); //cho phép s? d?ng ?i?n tr? kéo trong PORTB
wpub1=1; //Kéo ?i?n tr? trong t?i chân B1
wpub2=1;
wpub3=1;
wpub4=1;
wpub5=1;
set_tris_b(0xf0);
portb=0x0f;
lcd_putc("moi nhap pass");
trisc6=0;
trisc7=0;
//portc=0;
while(1)
{

set_tris_a(0xff);
set_tris_e(0);
if(button1==1)
{
c6=1;
c7=0;
xuatxung(125);
if (!(input(pin_a1))&&(button==0))
{
lcd_putc("\f");
button = 1; //Lock button
output_high(pin_e2);
xuatxung(20);
}
}
checkkey();
}
}
void init(){
//Timer 1 clock in = Fosc/4 = 5Mhz
//Speed of Speech = 344m/s ,Max time echo =25ms
//=> Max Distance of SRF05 = (344m/s*25ms)/2 = 4.3m
// Min time echo = 100us
//=> Min Distance of SRF05 = (344m/s*100us)/2 = 3.44cm
//=> Timer 1 with Ts = (1/5MHz)*Pre = 0.4uS (Prescaler = 2);
//=> Reg Timer 1 Circle
lcd_init();
setup_timer_1(T1_INTERNAL|T1_DIV_BY_2);
//Thi?t l?p Capture CCP1 dò xung c?nh lên
setup_ccp1(CCP_CAPTURE_RE);
enable_interrupts(INT_CCP1); // Cho phép ng?t khi CCP1 có tín hi?u
}
void trigger(){
trg = 0;
has_echo = 0;
TRISC2 = 0; //set chân RC2 làm output
RC2 = 1; //trigger
delay_us(20);
RC2 = 0;
TRISC2 = 1; //set chân RC2 làm input
setup_ccp1(CCP_CAPTURE_RE);
enable_interrupts(GLOBAL);
}
void xuatxung(int value)
{
//TRISC1 = 0;
setup_ccp2(ccp_PWM);
set_pwm2_duty(value);
setup_timer_2(t2_div_by_4,63,1);
}

bên dưới là file mô phỏng .