Detección de obstáculos mediante un sensor ultrasónico y un microcontrolador PIC

Estoy tratando de detectar obstáculos a una distancia específica usando un sensor ultrasónico que está conectado con un microcontrolador PIC.

• Sensor ultrasónico = HC-SR04
• Microcontrolador PIC = PIC16F628A

Esto encenderá un LED cuando se coloque un obstáculo frente al sensor ultrasónico a una distancia específica o una distancia menor que esa. Para hacer eso, he escrito el siguiente código ensamblador. Pero no parece estar funcionando como se esperaba. El LED no se enciende a ninguna distancia.

Aquí está el código de ensamblaje

PROCESSOR PIC16F628A
#INCLUDE <P16F628A.INC>
#INCLUDE <BANKSEL.INC>

CBLOCK 0x20 ; DEFINE VARIABLES USED
C1      
d1
d2
d3
ENDC

ORG 0x00     ;RESET VECTOR
GOTO MAIN


MAIN
Bank0       ;goto bank0
CLRF PORTA          ;Initialize PORTA by setting output data latches
MOVLW 0x07          ;Turn comparators off and
MOVWF CMCON         ;enable pins for I/O functions


Bank1
bsf TRISA,7  ;set RA7 as input (ECHOIN)
BCF TRISA,1  ;SET RA1 AS OUTPUT (LED)
BCF TRISA,0  ;SET RA0 AS OUTPUT  (TRIGGER PULSE)
;CLRF TRISB   ;PORTB pins defined as outputs

CLRF C1 ; NOW C1 VALUE IS '00000000'

Bank0


MOVLW b'00000000' ; CLEAR W REGISTER INITIALLY

LOOP
CALL TRIG          ;START TRIGGER PULSE
BTFSC PORTA,7      ;CHECK ECHO OUT PIN STATE
GOTO COUNT         ;ECHO OUTPUT IS HIGH
CALL CHECKCOUNT    ; THIS WILL CHECK THE VALUE IN C1
GOTO LOOP          

COUNT              ;THIS WILL COUNT THE TIME LENGTH OF ECHO PULSE
INCF C1,1          ;INCREASE C1 VALUE BY 1, SAVE IT IN C1
CALL DELAY         ;1mS DELAY
GOTO LOOP



CHECKCOUNT             ;THIS WILL CHECK THE 7TH BIT OF C1 REGISTER
BTFSC C1,7             ;IS 8TH BIT FILLED?
GOTO LEDON             ;YES
BCF PORTA,1            ;NO
L1
CLRF C1                ;CLEAR REGISTER TO WRITE NEXT COUNTED VALUE
RETURN

LEDON
BSF PORTA,1
GOTO L1


TRIG ;THIS WILL CREATE 10uS TRIG PULSE AT RB2 PIN WHICH IS CONNECTED TO TRIG PIN
     ; OF THE SENSOR
BSF PORTA,0
CALL DELAY10US
BCF PORTA,0
CALL DELAY60MS ;AVOID CREATING PULSE IN  t<50MS
RETURN




DELAY
; Delay = 0.001 seconds
; Clock frequency = 4 MHz

; Actual delay = 0.001 seconds = 1000 cycles
; Error = 0 %

            ;998 cycles
    movlw   0xC7
    movwf   d1
    movlw   0x01
    movwf   d2
Delay_0
    decfsz  d1, f
    goto    $+2
    decfsz  d2, f
    goto    Delay_0

            ;2 cycles
    goto    $+1
RETURN

DELAY10US
; Delay = 1e-006 seconds
; Clock frequency = 4 MHz

; Actual delay = 1e-006 seconds = 1 cycles
; Error = 0 %

            ;1 cycle
    nop

RETURN


DELAY60MS
; Delay = 0.06 seconds
; Clock frequency = 4 MHz

; Actual delay = 0.06 seconds = 60000 cycles
; Error = 0 %

                ;59998 cycles
    movlw   0xDF
    movwf   d1
    movlw   0x2F
    movwf   d2
Delay_1
    decfsz  d1, f
    goto    $+2
    decfsz  d2, f
    goto    Delay_1

            ;2 cycles
    goto    $+1

RETURN

END

¿Hay alguna modificación que hacer en este código para que funcione correctamente? ¿O alguien tiene un código más simple que este para hacer este tipo de trabajo? Gracias