Ausweichmanöver in ASM mit Ultraschallsensor

Hallo Community. Ich bin mommentan dabei die Software für ein Anfängerprojekt zu schreiben. Ich programmiere in Assembler. Ich bin erst vor kurzem wieder auf zur µC-Programmierung gekommen. Nun stellt sich mir folgende frage. Ich habe als hardware: Einen Mega8 notfalls auch einen Mega32 aber ich dencke der kleine wird für die Aufgabe mehr als genügen.
Ich habe mir ein SFR04 Ultraschalldistanzsensor bestellt welcher nächste Woche kommen müsste. Nun zur eigentlichen frage.

Ich steh mommentan auf dem schlauch wie ich es bewerkstelligen soll das der Roboter "intelligent" einem Hinderniss ausweicht. Er soll mehr oder weniger blöd durch die gegend fahren und wenn er ein Hinderniss entdeckt soll er diesem in der sinnvollsten art und weiße ausweichen. Es wird ein Runder Aufbau in der größe einer CD. Die Hardware wird in vorm von mehreren kleineren platinen umgesetzt welche mit Kabel oder stiftleisten verbunden werden.

Die Aktuelle Software stellt lediglich standarts zur verfügung:
Rückwärtsfahrt, vorwärts links rechts, links auf der stelle und rechts auf der stelle.
Der Ultraschallsensor wird an der Front fest angebracht. Er soll sich später mal per drehung orientieren und den besten "ausweg" aus seiner situation finden.

Code:

---

.include "m8def.inc".equ XTAL=1000000
.equ motorport=portb
.equ motorddr=ddrb
.equ motor1A=0
.equ motor2A=3
.equ motor3A=4
.equ motor4A=5
.equ motor1E=1
.equ motor2E=2
.def temp1=r16
.def statreg=r20
.equ hinderniss=0
.equ sensorddr=ddrb
.equ sensorport=portb
.equ sensorpin=pinb
.equ echo= 6
.equ trigger=7
.org 0x0000
rjmp init


.org 0x0020
init:
;;TIMER1 FAST PWM an OC1A und OC1B
;;
ldi temp1,LOW(RAMEND); Stack anlegen
out SPL,temp1 ;
ldi temp1,HIGH(RAMEND)
out SPH,temp1
;;FastPWM mode14 nötig also wgm 1110 Prescaler von 8
ldi temp1, low(1000)
out ICR1L, temp1
ldi temp1, high(1000)
out ICR1H, temp1
ldi temp1, (1<<WGM13)|(1<<WGM12)|(0<<CS12)|(1<<CS11)|(0<<CS10)
out TCCR1B, temp1
ldi temp1, (1<<WGM11)|(0<<WGM10)|(1<<COM1A1)|(0<<COM1A0)|(1<<COM1B1)|(0<<COM1B0)
;;Clear on compare set at bottom
out TCCR1A, temp1
;;PWM läuft OCR1A und B sind die umschaltstufen ICR1 der maximalwert
rcall stop
ldi temp1, low(800)
out OCR1AL, temp1
out OCR1BL, temp1
ldi temp1, high(800)
out OCR1AH, temp1
out OCR1BH, temp1
;;Motor wird auf 80% gepulsed, läuft aber nicht dar die PIns auf bremsen stehen


rjmp main






main:
;;;;HAUPTPROGRAMM;;;;
rcall vorwarts
rcall sensor1
sbrs statreg, hinderniss
rjmp main
;;HINDERNISS erkannt
rcall stop
;MOtoren gestoppt
rcall ausweg
rjmp main








ausweg:
;;Das ding steht
;;Timer0 prescaler 1024  wert 244 ist eine virtel sekunde
rcall links
ldi temp1, 0xFF
out TCCR0, temp1
;Timer läuft mit 1024
warten2:
in temp1, TCNT0
cpi temp1, 244
brlo warten2
rcall stop
rcall sensor1
sbrc statreg, hinderniss
rjmp ausweg
ret










;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;Sensor Ultraschall;;;;;;;;;;;;
;;;;;;;;;;;;;;;;;Routine gibt wahrheitswert auf statreg bit hinderniss aus
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;1 ist hinderniss näher 10cm


sensor1:
sbi sensorddr, trigger
;;;ca10µs impuls auf trigger
sbi sensorport, trigger
;HIGH
ldi temp1, 3
nop
wait1:
dec temp1
brne wait1


cbi sensorport, trigger
;LOW
;;Timer 0 als Counter mit prescaler 64  abstand 10cm is ca ein wert von 3


wait2:
sbis sensorport, echo
rjmp wait2
ldi temp1, 0x00
out TCNT0, temp1
ldi temp1, (1<<CS01)
out TCCR0, temp1        ;Ab jetz impulslänge zählen
wait3:
sbic sensorport, echo
rjmp wait3
;Timer stoppen
ldi temp1, (0<<CS01)
out TCCR0, temp1


in temp1, TCNT0
cpi temp1, 4
brlo setzen
cpi temp1, 4
brge rucksetzen
rcall wait4
ldi temp1, 0
out tccr0, temp1
out tcnt0, temp1
ret




setzen:
sbr statreg, hinderniss
ret


rucksetzen:
cbr statreg, hinderniss
ret




wait4: ;Wartezeit für Sensor 1000000/50 messungen pro sek max =20000
;;20000 takte müssen verbraten werden
;; Timer 0 erledigt da mit 256 prescaler und zählen bis 79
ldi temp1, 0
out tcnt0, temp1
ldi temp1, 0x05
out tccr0, temp1
;;TImer läuft
warten:
in temp1, tcnt0
cpi temp1, 79
brlo warten
ret
















;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;:::::::::::::::::::::::::::::::::::Motorroutine;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;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)


vorwarts:
;;1A und 3A low 2A und 4A high
ldi temp1, (1<<motor1A)|(1<<motor3A)|(0<<motor2A)|(0<<motor4A)
out motorddr, temp1
out motorport, temp1
ret


ruckwarts:
ldi temp1, (0<<motor1A)|(0<<motor3A)|(1<<motor2A)|(1<<motor4A)
out motorddr, temp1
out motorport, temp1
ret


links:
ldi temp1, (0<<motor1A)|(1<<motor3A)|(1<<motor2A)|(0<<motor4A)
out motorddr, temp1
out motorport, temp1
ret


rechts:
ldi temp1, (1<<motor1A)|(0<<motor3A)|(0<<motor2A)|(1<<motor4A)
out motorddr, temp1
out motorport, temp1
ret


stop:
ldi temp1, (1<<motor1A)|(1<<motor3A)|(1<<motor2A)|(1<<motor4A)
out motorddr, temp1
out motorport, temp1
ret

---

Nun weis ich einfach wirklich nicht wie ich das gnaze in software umsetzen soll. Typisch anfänger halt. Ist es notwenig eventuell noch drehzahlsensoren an den Rädern in form von Fototransistoren anzubringen. FUnktioniert das Auslesen des Sensors so?
mfg Daniel