Code:

//Make the 40KHz ping
#define PING_PORT PORTA.F0
#define PING_ON_DELAY  12
#define PING_OFF_DELAY 12

void ping(){
  //while(1){
  PING_PORT = 1;
  delay_us(PING_ON_DELAY);
  PING_PORT = 0;
  delay_us(PING_OFF_DELAY);

  PING_PORT = 1;
  delay_us(PING_ON_DELAY);
  PING_PORT = 0;
  delay_us(PING_OFF_DELAY);

  PING_PORT = 1;
  delay_us(PING_ON_DELAY);
  PING_PORT = 0;
  delay_us(PING_OFF_DELAY);

  PING_PORT = 1;
  delay_us(PING_ON_DELAY);
  PING_PORT = 0;
  //delay_us(PING_OFF_DELAY); }
}


#define interrupt_on() PIR1 = 0x00; PIE1 = 0x04; INTCON = 0xC0
#define interrupt_off() INTCON = 0x00;

unsigned char pokupio;
unsigned char skupljaj;

unsigned int T1;    //timer1 overflow

unsigned long T1_old;
unsigned long T1_new;

unsigned char bafer[25];

#define vd()  Vdelay_ms(100)
#define blink() Vdelay_ms(100)

void Init(){
  //init ports
  ADCON0 = 0;
  ADCON1 = 0;
  ANSEL  = 0;
  ANSELH = 0;

  TRISC = 1<<5; //RC5 is CCP1
  PORTC = 0;
  TRISB = 0;
  PORTB = 0;
  TRISA = 0;
  PORTA = 0;

  //init variables
  pokupio=0;
  skupljaj=0;
  T1 = 0;
  
  //init interrupts, timers
  CCP1CON = 0x05;   // 0000 0101 Capture mode every rising edge
  T1CON   = 0x01;   // 00 00 (prescaler 1:1) 000 1(turn on the timer1)

  T1_old  = 0;
  T1_new  = 0;
  

  //USART
  Usart_Init(9600);
  blink();
  Usart_Write(0x0D);
  Usart_Write(0x0A);

}


#define BAR0() PORTA.F2=1; PORTB |= 0x50; PORTC |= 0xDF
#define BAR1() PORTA.F2=0; PORTB |= 0x50; PORTC |= 0xDF
#define BAR2() PORTA.F2=0; PORTB |= 0x50; PORTC |= 0xDE; PORTC.F0=0
#define BAR3() PORTA.F2=0; PORTB |= 0x50; PORTC |= 0xDC; PORTC &= 0xFC
#define BAR4() PORTA.F2=0; PORTB |= 0x50; PORTC |= 0xD8; PORTC &= 0xF8
#define BAR5() PORTA.F2=0; PORTB |= 0x40; PORTB.F4 = 0; PORTC |= 0xD8; PORTC &= 0xF8
#define BAR6() PORTA.F2=0; PORTB &= 0xAF; PORTC |= 0xD8; PORTC &= 0xF8
#define BAR7() PORTA.F2=0; PORTB &= 0xAF; PORTC |= 0xC8; PORTC &= 0xE8
#define BAR8() PORTA.F2=0; PORTB &= 0xAF; PORTC |= 0xC0; PORTC &= 0xE0
#define BAR9() PORTA.F2=0; PORTB &= 0xAF; PORTC |= 0x40; PORTC &= 0x60
#define BAR10() PORTA.F2=0; PORTB &= 0xAF; PORTC &= 0x20


void prikazi(unsigned long x){

   if ( x > 256){
      Usart_Write(0xff);
      Usart_Write(0xff);
      BAR0();
      vd();
      return;
   }

  //posalji dva puta posto nekada receiver proguta prvi bajt
   Usart_Write(x & 0xff);
   Usart_Write(x & 0xff);

   if (x < 15) { // near
     BAR10();
   }else if (x < 20) {
     BAR9();
   }else if (x < 25) {
     BAR8();
   }else if (x < 30) {
     BAR7();
   }else if (x < 35) {
     BAR6();
   }else if (x < 40) {
     BAR5();
   }else if (x < 45) {
     BAR4();
   }else if (x < 50) {
     BAR3();
   }else if (x < 55) {
     BAR2();
   }else if (x < 60) {
     BAR1();
   }else{
     BAR0();
   }

   vd();
}


void main(){
  unsigned long daljina;

  Init();
  daljina = 0;
  pokupio = 0;
  while(1){
    interrupt_off(); //should be off already but for just in case
    Vdelay_ms(200);
    skupljaj = 1;
    
    T1 = 0;
    T1_old = (TMR1H << 8) | TMR1L;
    T1_new = 0;
    
    ping();
    interrupt_on();
    prikazi(daljina); //ovo traje neko vreme
    interrupt_off();
    
    if (pokupio){
      pokupio = 0;

      // 8MHz crystal so 2000000 ticks in second
      // speed of air at 20deg 340m/s
      // noTicks * 340 / 2000000 = double distance in m
      // (noTicks * 340 / 2000000)*100/2 = distance in cm
      // distance in cm = noTicks * 34 / 4000 = noTicks *17/2000

      if (T1_new < T1_old){ //greska, odsjaj sa strane ili tako nesto
       //nothing1();
        continue;
      }
      
      daljina = T1_new - T1_old;
      daljina *= 17;
      daljina /=2000;
    } else {
      daljina = 32000;
      //nothing();
      continue;
    }
  }
}

void interrupt(){
  if (PIR1 & (1<<TMR1IF)) { // timer1 overfow
    PIR1 &= ~(1<<TMR1IF);   // clear timer1 interrupt flag
    T1++;
  }
  if (PIR1 & (1<<CCP1IF)) { // we captured the echo
    PIR1 &= ~(1<<CCP1IF);
    if (skupljaj){ // if we were waiting for the echo
      skupljaj = 0; //we do not expect another one
      pokupio = 1;
      T1_new = (T1 << 16) | (CCPR1H << 8) | CCPR1L;  // on capture TMR1x is copied into CCPR1x
    }
  }  
}