Что не так с этим примером PIC16F877A HCSR04?

Question

Я работаю над датчиком расстояния с PIC16F877A. Я использую компилятор MPLAB IDE И XC8. Моя цель - постепенно включать правые светодиоды на определенном расстоянии. но светодиоды мигают нестабильно. Что я делаю неправильно? расстояние / 5 При измерении 5 см это 5/5, результат - 1 и горит 1 светодиод. Когда он измеряет 10 см, это 10/5, а результат - 2 и горят 2 светодиода. Когда вы измеряете 15 см, это 15/5, и в результате горят 3 и 3 светодиода. Когда вы измеряете 20 см, это 20/5, и результат 4 и 4 светодиода горят. Если расстояние меньше 5 см или больше 20 см, светодиоды не горят.

https://stackru.com/images/095f2a095e8ac8ae7dc224449e1e9e192ade3f35.png

      #include <xc.h>
#include <pic16f877a.h> 
#pragma config FOSC = XT        // Oscillator Selection bits (XT oscillator)
#pragma config WDTE = OFF       // Watchdog Timer Enable bit (WDT disabled)
#pragma config PWRTE = OFF      // Power-up Timer Enable bit (PWRT disabled)
#pragma config BOREN = OFF      // Brown-out Reset Enable bit (BOR disabled)
#pragma config LVP = OFF        // Low-Voltage (Single-Supply) In-Circuit Serial Programming Enable bit (RB3 is digital I/O, HV on MCLR must be used for programming)
#pragma config CPD = OFF        // Data EEPROM Memory Code Protection bit (Data EEPROM code protection off)
#pragma config WRT = OFF        // Flash Program Memory Write Enable bits (Write protection off; all program memory may be written to by EECON control)
#pragma config CP = OFF         // Flash Program Memory Code Protection bit (Code protection off)
#define _XTAL_FREQ 4000000
#define trigger RC2
#define echo RC3

int calc_distance();

int calc_distance()
{
    int distance=0;
     
    trigger=1;
    __delay_us(10);
    trigger=0;
    
    while(!echo); // normally the echo pin is logic 0. If we toggle the zero and throw it into the while, it returns idle until the echo arrives (until the signal becomes logic 1).
    TMR1ON=1;//In the upper loop, while returning the command empty, echo comes and exits the upper loop and timer1 is activated.
    
    while(echo);
    TMR1ON=0;//
 
  distance=TMR1/58;
  return distance;
    
}



void main(void)
{
 
  int dist=0; // Create Distance Variable
  
 
  TRISB = 0x00;  // Set PORTB To Be Output Port (All The 8 Pins)
  
  PORTB = 0x00;  // Set PORTB To Be LOW For initial State
 
  TRISC2 = 0;     // Set RC2 To Be Output Pin ( Trigger )
  RC2 = 0;
  
  TRISC3 = 1;   // Set RC3 To Be Input Pin ( Echo )
  
  //--[ Configure Timer Module To Operate in Timer Mode ]--
  // Clear The Pre-Scaler Select Bits
  T1CKPS0=0;
  T1CKPS1=0;
  
  TMR1CS=0;// Choose The Local Clock As Clock Source
  
  
  while(1)
  {
    calc_distance();
    dist = calc_distance()/5;
    if(dist==1)
    {PORTB = 0x01; __delay_ms(50);}
    if(dist==2)
    {PORTB = 0x03; __delay_ms(50);}
    if(dist==3)
    {PORTB = 0x07; __delay_ms(50);}
    if(dist==4)
    {PORTB = 0x0F; __delay_ms(50);}
    else
    {PORTB = 0x00; __delay_ms(50);}
  }
  return;
}