PIC Vietnam - [help] trợ giúp robot dò đường

tình hình là mình vừa viết xong co de cho robot dò đường. khi test tường khối dò đường và khối hiển thị riêng lẻ thì ok nhưng test 2 khối cùng lúc thì nó báo lỗi. cao thủ vào giúp cái.
code và mô phỏng của mình đây
#include <16F877A.h>
#fuses NOWDT,HS,NOPUT,NOPROTECT,NODEBUG,NOBROWNOUT,NOLVP
#use delay(clock=20000000)
/* ÐINH NGHIA CÁC CHÂN VÀ PORT*/
#include <def_877a.h>
#include <lcd_4bit.c>
#define dir_left RB3
#define dir_right RB2
#define SENSOR PORTD

long int16 pulse_encoder_right=0,pulse_encoder_left=0;
long int16 vantoc_left=0,vantoc_right=0;
int1 enable_display=0; // Co hien thi LCD

#INT_TIMER1 ///chuong trinh ngat timer 1
void Motor_Speed()
{
disable_interrupts(INT_EXT);
setup_timer_1(T1_DISABLED);
vantoc_right=(pulse_encoder_right);//*1000)/(334*3);
pulse_encoder_left=get_timer0(); // Lay gia tri trong cac thanh ghi cua counter
vantoc_left=(pulse_encoder_left);//*1000)/(334*3);
pulse_encoder_right=0;
set_timer0(0);
set_timer1(0xF15A);
setup_timer_1(T1_INTERNAL|T1_DIV_BY_4);
enable_interrupts(INT_EXT);
enable_display=1;
}

#INT_EXT //chuong trinh ngat ngoai
void Count_Pulse()
{
pulse_encoder_right++;
}

/* CHUONG TRÌNH CHÍNH */

int left=0;
int right=0;
void speed(int speed_left,int speed_right)//Dieu khien dong co
{

if(speed_left >= 0) {dir_left=0;left=2.51*speed_left;}
else {dir_left=1;left=2.51*( -speed_left);}
if(speed_right>=0) {dir_right=0;right=2.51*speed_right;}
else {dir_right=1;right=2.51*( -speed_right);};
set_pwm1_duty(left);
set_pwm2_duty(right);
}

void main ()
{
TRISC=0; // PORTC là ngõ ra ( dOg co)
TRISD=255;// PORTD là ngõ vào cam bien
TRISE=0;
TRISB=0x01;
TRISA=255;

enable_interrupts(GLOBAL); // Cho phep ngat toan cuc
ext_int_edge(H_TO_L); // Cau hinh ngat ngoai theo canh xuong
enable_interrupts(INT_EXT); // Cho phep ngat ngoai
enable_interrupts(INT_TIMER1); // Cho phep ngat tran TIMER1

setup_timer_0(RTCC_EXT_H_TO_L|RTCC_DIV_1|RTCC_8_BI T);//Tang gia tri timer0 len 1 khi co 1 xung xuong tai chan PIN_A4 (T0CKI)
set_timer0(0); // Gan gia tri ban dau cho counter

// Thiet lap TIMER1: Dinh thi 3ms
setup_timer_1(T1_INTERNAL|T1_DIV_BY_4); // Cau hinh bo chia cho TIMER1
set_timer1(0xF15A); // Cau hinh gia tri cho thanh ghi TMR1 - Dinh thi 3ms

////////////////////////////
setup_timer_2(T2_DIV_BY_16,250,1);
setup_ccp1(CCP_PWM);
set_pwm1_duty(0); // DK dog co trai
setup_ccp2(CCP_PWM);
set_pwm2_duty(0);//DK dong co phai
RB5=1; //Cho dong co chay
RB4=1;

///////////// LCD
lcd_init();
hangcot(1,1);
hienthi("DO AN TOT NGHIEP");
delay_ms(300);
hangcot(2,1);
hienthi("NGUYEN MINH TIEN");
delay_ms(300);
xoamanhinh();
hienthi("ROBOT DO DUONG");
hangcot(2,1);
hienthi("SU DUNG PIC16F877A");
delay_ms(300);
xoamanhinh();

while(1)
{
switch (SENSOR)
{
case 0b11100111: speed(40,40); break;
case 0b11110011: speed(40,30); break;
case 0b11111001: speed(50,25); break;
case 0b11111100: speed(60,20); break;
case 0b11111110: speed(65,15); break;
case 0b11001111: speed(30,40); break;
case 0b10011111: speed(25,50); break;
case 0b00111111: speed(20,60); break;
case 0b01111111: speed(15,65); break;
/*case 0b00011111: speed(0,70) ; break;
case 0b00001111: speed(0,70) ; break;
case 0b00000111: speed(0,70) ; break;
case 0b00000011: speed(0,70) ; break;
case 0b00000001: speed(0,70) ; break;
case 0b11110000: speed(70,0) ; break;
case 0b11100000: speed(70,0) ; break;
case 0b11000000: speed(70,0) ; break;
case 0b10000000: speed(70,0) ; break; */
default : speed(60,90);break;
}
// xu ly toc do tra ve hien thi len LCD
if(enable_display==1)
{

hangcot(1,1);
printf(hienthi,"V=%2lu v/s",vantoc_right);
hangcot(2,1);
printf(hienthi,"V=%2lu v/s",vantoc_left);
enable_display=0;
}
}
}