4 con srf05 + pic

[sonitwilli]

Mọi người xem thử code này bị sao mà chạy hok dc vậy?

#define DIR_LEFT PIN_D0
#define EN_LEFT PIN_D1
#define DIR_RIGHT PIN_D2
#define EN_RIGHT PIN_D3

#define SRF04_TRIGGER1 PIN_B1
#define SRF04_ECHO PIN_B0
#define NO_OBJECT 0

#INT_EXT
void Ngat_Ngoai(void)
{
disable_interrupts(GLOBAL);
num_pulse+=get_timer1();
range_ok=1;
enable_interrupts(GLOBAL);
}

#INT_TIMER1
void Ngat_Timer1(void)
{
disable_interrupts(GLOBAL);
num_pulse+=0xffff;
enable_interrupts(GLOBAL);
}

void SRF05_StartRange()
{
while(!range_ok)
{
output_high(SRF04_TRIGGER1);
delay_ms(15); // Phai tao 1 xung len co do lon it nhat 10ms
output_low(SRF04_TRIGGER1); // Bat dau phep do.
while(!(input(SRF04_ECHO))); // Doi cho den khi chan ECHO duoc keo len cao
set_timer1(0);
enable_interrupts(GLOBAL);
delay_ms(50);
}
}

float32 SRF05_GetDistance()
{
float32 time_us=0, distance=0;
SRF05_StartRange();
disable_interrupts(GLOBAL);
if(num_pulse>180000)
{
num_pulse=0;
range_ok=0;
return NO_OBJECT;
}
else
{
time_us=num_pulse/6;
distance=time_us/58;
num_pulse=0;
range_ok=0;
return distance;
}
}

//================================================== ===
void dithang()
{
output_high(DIR_LEFT);
output_low(EN_LEFT);
output_high(DIR_RIGHT);
output_low(EN_RIGHT);
delay_ms(200);
}
void chinhphai()
{
output_low(DIR_LEFT);
output_low(EN_LEFT);
output_high(DIR_RIGHT);
output_low(EN_RIGHT);
delay_ms(200);
}
void chinhtrai()
{
output_high(DIR_LEFT);
output_low(EN_LEFT);
output_low(DIR_RIGHT);
output_low(EN_RIGHT);
delay_ms(200);
}

void xuli(int16 x)
{
if(x >= 19 && x <= 21) dithang();
else if(x > 21) chinhtrai();
else chinhphai();
}
//================================================== ====

void main()
{
float32 range;
output_float(SRF04_ECHO);
output_drive(SRF04_TRIGGER1);
port_b_pullups (TRUE); //Thiet lap ngo vao port B pullup(dien tro keo len)
ext_int_edge(H_TO_L); // ngat canh xuong
setup_timer_1(T1_INTERNAL|T1_DIV_BY_1); //F_TIMER1=F_OSC/4
enable_interrupts(INT_TIMER1);
enable_interrupts(INT_EXT); // kich hoat ngat ngoai
disable_interrupts(GLOBAL);
while(TRUE)
{
range=SRF05_GetDistance();
xuli(range);
delay_ms(500);
}
}