Problem mit SR04 Ultraschalsensor in Assembler

Hallo Community. Ich habe momentan ein kleines Projekt vor mir. Einen kleinen miniaturbot welcher einfach nur "blöd" in der Pampa rumfahren soll und möglichen Hindernissen ausweichen. Das Ganze ist mit einem tiny2313 realisiert. Anfangs waren es 3 IR Sensoren (vorne, links, rechts) Mit welchen es auch relativ gut funktioniert hat. Das Problem waren nur Steigungen. Wenn ich ihn im freien auf die Welt Losgelassen habe hat er bei jedem kleinen Hubbel und bei jedem Grashalm der länger als 5 cm war sofort aufgegeben und den Rückzug angetreten. Das ist allerdings nicht sinn der Sache. Mein Assembler Code mag wüst und chaotisch sein. Trotzdem bitte ich euch in einmal genauer unter die Lupe zu nehmen dar das ein "Von Tuten und Blasen noch keine Ahnung, Aber schauen wir mal obs iwie funktioniert" Projekt ist. Motoren habe ich 2 "China"-Miniaturgetriebemotoren, die allerdings bei 5 V schon wirklich gut Kraft bei ca 50Umdrehungen an den Tag legen. Motortreiber ist ein LM298N. Infrarotsensoren sind FC51 von Zitat:" shai3c.com". Der unterbau besteht aus einer Runden CD-Spindel auf deren Unterseite die motoren, ein Stützrad und 3 IR-Sensoren verbaut sind.
Nun soll es so umgesetzt werden das der vordere IR-Sensor wegfällt und durch einen Ultraschallsensor ersetzt wird, Dar die beeinträchtigung durch Tageslicht doch nicht zu vernachlässigen ist. Später soll das ganze noch mit einer einrichtung um zufällig gefundene Linien zu folgen ergänzt werden. Auch wäre es ein Wunschtraum wenn er später einmal einem bewegenden Hinderniss folgen könnte. Auch ist eine Umfangreiche Beleuchtung mit Hilfe eines Schieberegisters geplant.

Nun wenn ihr den Code seht werden sich euch die Haare streuben.

Code:

---


.org 0x000
rjmp main
.equ motorport=portb
.equ motorddr=ddrb
.equ m1a=0
.equ m2a=1
.equ m3a=2
.equ m4a=3
.equ sensorport=portb
.equ sensorddr=ddrb
.equ sensorpin=pinb
.equ irl=5
.equ irr=4
.equ irm=6
.equ trigger=6
.equ echo=7
.org 0x020
.def temp1=r16
.def weg=r23
.def temp2=r21
.def temp3=r22
.equ XTAL=1000000
.include "tn2313def.inc"
main:
powerupwait:
ldi r16, low(RAMEND)
out SPL, r16

    ldi  r18, 16
    ldi  r19, 57
    ldi  r20, 14
L16: dec  r20
    brne L16
    dec  r19
    brne L16
    dec  r18
    brne L16
rcall initm
go:


rcall vor
rcall sensorl
rcall vor
rcall sensorr
rcall vor
;rcall sensorm
rcall vor
rcall sensorl
rcall vor
rcall sensorr
rcall vor
;rcall sensorm
clr r26
rcall sensorus
cpi weg, 5
brlo hindernisvorne2
; 200ms at 1.0 MHz


;rcall irsensor
rjmp go


;;IRSENSOREN
sensorm:
in r16, sensorpin
sbrc r16, irm
ret
;HINDERNISS LINKS
rcall stop
rcall ruck
; Delay 500 000 cycles
; 500ms at 1.0 MHz


    ldi  r18, 6                                                                                                                                                                                                                                                             
    ldi  r19, 138
    ldi  r20, 86
L11: dec  r20
    brne L11
    dec  r19
    brne L11
    dec  r18
    brne L11
        rcall links
        ldi r16, 255
        marker01:
        dec r16
        brne marker01
rcall links
    ldi  r18, 5
    ldi  r19, 138
    ldi  r20, 86
L12: dec  r20
    brne L12
    dec  r19
    brne L12
    dec  r18
    brne L12
ret


sensorl:
in r16, sensorpin
sbrc r16, irl
ret
;HINDERNISS LINKS
rcall stop
sbrs r26, 0
rcall rechts
sbrc r26, 0
rcall ruck
; Delay 500 000 cycles
; 500ms at 1.0 MHz


    ldi  r18, 3
    ldi  r19, 38
    ldi  r20, 86
L1555: dec  r20
    brne L1555
    dec  r19
    brne L1555
    dec  r18
    brne L1555
rcall stop
ret
hindernisvorne2:
rcall hindernisvorne
ret
sensorr:
in r16, sensorpin
sbrc r16, irr
ret
;HINDERNISS LINKS
rcall stop
sbrs r26, 1
rcall links
sbrc r26, 1
rcall ruck
; Delay 500 000 cycles
; 500ms at 1.0 MHz


    ldi  r18, 3
    ldi  r19, 38
    ldi  r20, 86
L2: dec  r20
    brne L2
    dec  r19
    brne L2
    dec  r18
    brne L2
rcall stop
ret






irsensor:
sbi sensorport, trigger
nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop
cbi sensorport, trigger
wait1:
sbis sensorpin, echo
rjmp wait1


;;Timer0 Als counter misbrauchen um dielänge ds signals zu erfassen
;;0,5ms entsprechen 10 cm distanz das sind bei 1mhz takt 500 takte. genauer gesagt 583 laut datenblatt
;;prescaler von 256 würde also ausreichen...das würde einen counterwert von 583/256=2 ergeben..und bei max distanz von 68
;;
;CS02 CS01 CS00 Description
;0 0 0 No clock source (Timer/Counter stopped)
;0 0 1 clkI/O/(No prescaling)
;0 1 0 clkI/O/8 (From prescaler)
;0 1 1 clkI/O/64 (From prescaler)
;1 0 0 clkI/O/256 (From prescaler)
;1 0 1 clkI/O/1024 (From prescaler)
;1 1 0 External clock source on T0 pin. Clock on falling edge
;1 1 1 External clock source on T0 pin. Clock on rising edge
ldi temp1, (1<<CS02)
out tccr0, temp1        ;prescaler auf 256
;;AB JETZT LÄUFT DER TIMER


warten2:        ;Timer stoppten wenn Echo wieder Low ist
sbic sensorpin, echo
rjmp warten2
ldi temp1, 0x00
out tccr0, temp1
;;Timer 0 gestoppt
in temp1, tcnt0
cpi temp1, 3
brlo hindernisvorne
ret


hindernisvorne:
rcall stop
rcall ruck
    ldi  r18, 3
    ldi  r19, 138
    ldi  r20, 86
L3: dec  r20
    brne L3
    dec  r19
    brne L3
    dec  r18
    brne L3
rcall stop
rcall sensorl
rcall sensorr
rcall vor
ret






sensorus:
sbi sensorddr, trigger
sbi sensorport, trigger
ldi temp1, (XTAL/1000000+2)
trig:


nop nop nop nop nop nop nop nop nop nop nop
dec temp1
brne trig
cbi sensorport, trigger
;;;;GETRIGGERT ABHÄNGIG VOM TAKT


;;;117 takte bei 1 Mhz sind 1 cm
;;;Auf Echo warten
wait1222:
cbi sensorddr, echo
in temp1, sensorpin
clr weg
sbrs temp1, echo
rjmp wait1222


;;So nun fix mit dem zählen anfangen
zahlen:


ldi temp2, (XTAL/1000000)
marker02:
ldi temp1, (116/2)
marker011:
dec temp1
brne marker011
dec temp2
brne marker02
inc weg
;;;So nun wäre es 1 cm
in temp1, sensorpin
sbic sensorpin, echo
rjmp zahlen
ret






;MOTOREN
;Eingang "1A"        Eingang "2A"        "1,2EN"        Ausgang "1Y"        Ausgang "2Y"        entspricht
;0V        0V        5V        -        -        Motor kurzschließen (Bremsen)
;0V        5V        5V        -        +        Motor dreht vorwärts
;5V        0V        5V        +        -        Motor dreht rückwärts
;5V        5V        5V        +        +        Motor kurzschließen (Bremsen)
;egal        egal        0V        hochohmig        hochohmig        Motor aus (Verbindung trennen, nicht kurzschließen)
stop:
initm:
ldi r16, (1<<m1a) |(1<<m2a)|(1<<m3a)|(1<<m4a)
out motorddr, r16
out motorport, r16
ret
vor:
ldi r16, (0<<m1a) |(1<<m2a)|(0<<m3a)|(1<<m4a)
out motorport, r16
ret
ruck:
ldi r16, (1<<m1a) |(0<<m2a)|(1<<m3a)|(0<<m4a)
out motorport, r16
ret
links:
sbr  r26, 1
ldi r16, (1<<m1a) |(0<<m2a)|(0<<m3a)|(1<<m4a)
out motorport, r16
ret
rechts:
sbr r26, 0
ldi r16, (0<<m1a) |(1<<m2a)|(1<<m3a)|(0<<m4a)
out motorport, r16
ret

---

Ich hoffe einer von Euch erbarmt sich dieses Chaos durchzuarbeiten. Ich freue mnich über jeglich Tipps und Verbesserungen eurer seits.

MFG Daniel