Problema con HC-SR04 y PIC18F2550. XC8, MPLAB X IDE

[MiRmcHH]

Buenos días, soy nuevo en el tema de los microcontroladores, solo he hecho un par de aplicaciones y todas con el PIC18F2550. Por ahora quisiera hacer una simple con el sensor ultrasónico que sea capaz de medir el nivel del agua en un tanque pero el problema es que el sensor no me esta midiendo nada, probe el sensor en una aplicación con un arduino para descartar la idea de que este malo, y si funciona muy bien, he probado el microcontrolador para otras aplicaciones y tambien me funciona bien. He visto varios tutoriales y la programación es bastante sencilla, no entiendo porque no me funciona. A continuación les muestro el código:

Código: C

1. #include <xc.h>
2. #include <stdio.h>
3.  
4. #define _XTAL_FREQ 48000000 //48MHz frecuencia interna
5.  
6. #define led1 LATAbits.LATA5
7. #define pEcho PORTBbits.RB2
8. #define lTrig LATBbits.LATB1
9.  
10. void main(void) {
11.  
12.     ADCON1bits.PCFG=0b1111; //Todos los puertos son digitales
13.  
14.     TRISAbits.RA5 = 0; //RA5 como salida
15.     TRISBbits.RB2 = 1; //RB2 como entrada
16.     TRISBbits.RB1 = 0; //RB1 como salida
17.    
18.     T0CONbits.T08BIT=0; //Timer0 seleccionado para registro de 16bits
19.     T0CONbits.T0CS=0; //Timer0 utiliza ciclo interno del reloj micro Fosc/4
20.     T0CONbits.PSA=0; //Activo prescaler del timer0
21.     T0CONbits.T0PS=0b000; //Prescaler de 1:2 - Tiempo de conteo = (4/48M)*2 = 0,16us
22.     INTCONbits.TMR0IE=0; //Desactivada la interrupción por desbordamiento del timer0
23.     INTCONbits.TMR0IF=0; //Limpiar la bandera por desbordamiento del timer 0
24.  
25.     int conteo = 0; //Variable para registro del conteo total del pulso Echo del sensor 1
26.     long distancia = 0; //Variable para registro de la distacia medida por el sensor 1
27.  
28.     while(1){
29.        
30.         TMR0H = 0; //Limpiar Registro alto del timer0
31.         TMR0L = 0; //Limpiar Registro bajo del timer0
32.  
33.         lTrig = 1; //Enviamos pulso Trig del sensor HC-SR04
34.         __delay_us(10); //definimos ancho del pulso 10us
35.         lTrig = 0; //Apagamos el pulso
36.  
37.         while(!pEcho); //Esperamos que le llegue señal al Echo
38.         TMR0ON = 1; //Encendemos el timer0
39.         while(pEcho); //Esperamos que el Echo deje de recibir señal
40.         TMR0ON = 0; //Apagamos el timer0
41.  
42.         conteo = (TMR0L | (TMR0H<<8)); //Se guarda el registro en la variable
43.  
44.         distancia = ((conteo)/58); //Distancia en cm
45.  
46.         if(distancia <= 10){
47.             led1=!led1;
48.             for(int i=1;i<=100;i++) __delay_ms(10);
49.         }else led1=0;
50.     }
51.     return;
52. }

Si alguien encuentra algún error en el código lo agradecería. La idea es que si el sensor detecta "algo" a una distancia menor o igual a 10cm entonces encienda un led intermitente, de lo contrario que el led este apagado.

No se si es relevante esta información pero El PIC18F2550 lo programo con el HIDBootlader.

De ante mano, muchas gracias.