Hinderniserkennung mit einem Ultraschallsensor und einem PIC-Mikrocontroller

Ich versuche, Hindernisse in einer bestimmten Entfernung mit einem Ultraschallsensor zu erkennen, der mit einem PIC-Mikrocontroller verbunden ist.

• Ultraschallsensor = HC-SR04
• PIC-Mikrocontroller = PIC16F628A

Dadurch wird eine LED eingeschaltet, wenn sich ein Hindernis in einem bestimmten Abstand oder einem geringeren Abstand vor dem Ultraschallsensor befindet. Dazu habe ich folgenden Assemblercode geschrieben. Aber es scheint nicht wie erwartet zu funktionieren. Die LED leuchtet bei keiner Entfernung.

Hier ist der Assembler-Code

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

Müssen Änderungen an diesem Code vorgenommen werden, damit er ordnungsgemäß funktioniert? Oder hat jemand einen einfacheren Code als diesen, um diese Art von Arbeit zu erledigen? Danke