ĐO khoảng cách dùng HC-SR04 đơn giản, hiệu quả

[pstin]

#include <main.h>
#include <def_88.h>
#include <ctype.h>
#include <stddef.h>
#use fast_io(B)

Void hien_thi();

#define LCD_ENABLE_PIN PIN_B1
#define LCD_RS_PIN PIN_B0
#define LCD_RW_PIN PIN_B2
#define LCD_TYPE 2
#define LCD_DATA4 PIN_B4
#define LCD_DATA5 PIN_B5
#define LCD_DATA6 PIN_B6
#define LCD_DATA7 PIN_B7
#define trig pin_A3
#define echo pin_A2
#include <lcd.c>

int16 distance, d1, d2, time;

void main()
{
setup_oscillator(OSC_4MHZ);
setup_spi(SPI_SS_DISABLED);
setup_timer_1(T1_INTERNAL|T1_DIV_BY_8);
setup_comparator(NC_NC_NC_NC);
setup_vref(FALSE);
set_tris_B(0x08);
set_tris_A(0xE3);
lcd_init();
lcd_gotoxy(1,1);
lcd_putc("MEASURE DISTANCE");
delay_ms(1500);
lcd_gotoxy(1,2);
lcd_putc("USE HC-SR04 SENS");
delay_ms(1500);
lcd_refresh();

while(true)
{
output_high(trig);
delay_us(20);
output_low(trig);
while(!input(ECHO))
{}
set_timer1(0);
while(input(ECHO))
{}
time=get_timer1();
distance=time*0.138 + 1.1 ; // Calculating the distance time/58 = 1/4*4*8/58=0.138)
hien_thi();

}

}

void hien_thi()
{
if(distance>550)
{
lcd_gotoxy(1,1);
lcd_putc("NO OBJECT < 0.5M");
}
else
{
lcd_gotoxy(1,1);
lcd_putc("DISTANCE:");
d1=distance/100;
d2=distance%100;
lcd_gotoxy(11,1);
lcd_putc(d1/10+0x30);
lcd_gotoxy(12,1);
lcd_putc(d1%10+0x30);
lcd_gotoxy(13,1);
lcd_putc(d2/10+0x30);
lcd_gotoxy(14,1);
lcd_putc(d2%10+0x30);
lcd_gotoxy(15,1);
lcd_putc("cm");
delay_ms(500);
}
}