Odometrie für Asuro in Assembler, brauche Hilfe

Hallo,
Ich habe mir gedacht, den Asuro mit Assembler zu Programmieren, na ja ich schwimme
immer gegen den Strom...
Es klappt bei den ersten Versuchen, aber bei Odometrie stoße ich doch auf Probleme, und ich hoffe, daß mir hier ein Assemblerguru vielleicht weiterhelfen kann...
zu meinem Programm,
Der Assuro fährt einfach geradeaus, bis der irgendwo gegen fährt, dann fährt er eine Sekunde zurück, dreht sich eine halbe sekunde und fährt weiter...
Naja es klappt alles soweit bis auf das geradeausfahren,
Meine Routine will einfach nicht funktionieren wenn ihr mal drüber gucken könnt,
ich hoffe, daß ich alles ausreichend komentiert habe

Code:

---

.include "m8def.inc"

;Motorenrichtung
.equ fwd = PB5                ;vor
.equ rwd = PB4                ;zurück
;Ende Motorenrichtung


;Definiere Anfangwert vom Zähler
.equ anfang = 256 - 255

;Definiere 5 Sekunden
.equ fuenfsek = 153
;Ende Definition von 5 Sekunden
;Definiere 1 Sekunde
.equ einesek = 32
;Definiere halbe sekunde
.equ halbesek = 16
.def Lold=R1        ;Alter Wert Odometrie links
.def Rold=R2        ;Alter Wert Odometrie rechts
.def Lnow = R3        ;Neuer Wert Odometrie links
.def Rnow = R4        ;Neuer Wert Odometrie rechts
.def tmp = R16        ;Multipurose
.def tmpi = R17 ;Multipurose für Interrupts
.def zaehler = R18 ; Zählerregister
.def statusmot = R19 ;Motorenstatus
.def zaehlerodo = R20 ;Zähler Odometrie
;bit 0 -> Motor Rechts 0 vor 1 zurück
;bit 1 -> Motor links  0 vor 1 zurück 
.def geschrechts = R24        ;Geschwindigkeit für Motor Rechts
.def geschlinks = R25;Geschwindigkeit für Motor Links

;Vektoren
.org 0x000
        rjmp reset        ;reset
.org INT1addr
        rjmp kolision
.org OVF0addr                ;Timer0
        rjmp timer       

;Ende Vektoren


reset:
        ;Stack einrichten
        ldi tmp,HIGH(RAMEND)
        out SPH,tmp
        ldi tmp,LOW(RAMEND)
        out SPL,tmp
        ;PWM einstellen
        ldi tmp,(1<<WGM10) | (1<<COM1A1) | (1<<COM1B1)
        out TCCR1A,tmp
        ldi tmp,(1<<CS11)
        out TCCR1B,tmp
        ;A/D Conversion
        ldi tmp,(1<< ADEN) | (1<<ADPS2) | (1<<ADPS1)
        out ADCSRA,tmp
        ldi tmp,(1<<ADLAR) | (1<<REFS0)
        out ADMUX,tmp
        ;Timer 0 einstellen
        ldi tmp,(1<<CS02) | (1<<CS00)        ;prescaler 1024
        out TCCR0,tmp
        ldi tmp,(1<<TOIE0)        ;Interrupt bei überlauf
        out TIMSK,tmp
       
        ;DDR für Tastenabfrage               
        cbi DDRD,PD3
        ;Ende DDR für Tastenabfrage
        ;Einstellen Low Level Interrupt für Tastenabfrage
        ldi tmp,(1<<ISC11)
        out MCUCR,tmp
        ldi tmp,(1<<INT1)
        out GICR,tmp
        ;Ende EXT1 Interrupt
        ; DDR für Motor Rechts und Statusled green
        ldi tmp,(1<<PB0) |(1<<PB1) | (1<<PB2) | (1<<PB5) | (1<<PB4)
        out DDRB,tmp
        ;DDR für Motor Links,Statusled red, LED für Linienverfolgung und LEDs
        ;Odometrie und Backleds
        ldi tmp,(1<<PD2) | (1<<PD4) | (1<<PD5) | (1<<PD6) | (1<<PD7)
        out DDRD,tmp
        ldi zaehler,0x00 ; lösche inhalt von zähler
        rcall motorenloeschen
        sei
main:
        ;Schalte Liniendiode ein...
        rcall lineon
               
        ;Lade Wert für vorwärtsfahren
        sbi PORTB,fwd        ;in den rechten
        sbi PORTD,fwd        ;und in den linken Motor
        ldi geschrechts,0x90        ;Geschwindigkeit 0x90
        ldi geschlinks,0x90
       
        rcall motorengesch        ;LOS!
                        ;Starte Timer für 5 Sekunden
       
loop:
       
        cpi statusmot,0x80        ;liegt Kolision vor ?
        brne odometrie        ;wenn nicht dann mit odometrie weitermachen
        ldi statusmot,0x00
        rcall motorenloeschen        ;Stop
        ;Beide Motoren zurück
        sbi PORTB,rwd
        sbi PORTD,rwd
        ldi zaehler,einesek
        ldi tmp,anfang
        out TCNT0,tmp        ;Timer0 auf eine Sekunde       
warte:       
        tst zaehler
        brne warte
        rcall motorenloeschen
        sbi PORTB,rwd
        sbi PORTD,fwd
        ldi zaehler,halbesek
       
        ldi tmp,anfang
        out TCNT0,tmp
       
warte_:
        tst zaehler
        brne warte_
        rcall motorenloeschen
        sbi PORTB,fwd
        sbi PORTD,fwd       
        rjmp loop
       
odometrie:
        cbi DDRC,PC0        ;schalte PC0 als eingang
        cbi DDRC,PC1        ;schalte PC0 als eingang
       
        sbi PORTD,PD7 ;schalte Odometrie LEDs ein
        mov Lold,Lnow        ;sichere alten Zustand
        ;lese linkes Rad in tmp
       
       
        sbi ADMUX,MUX0
        cli
        sbi ADCSRA,ADSC
AD_BUSYL:
        sbis ADCSR,ADIF
        rjmp AD_BUSYL
       
        in tmp,ADCL
        in tmp,ADCH
        sei
                               
        cpi tmp,0xA0 ;vergleiche Ergebnis mit 0xA0
        brlo schwarzL        ;wennkleiner springe nach schwarz
        rjmp weissL
schwarzL:
        ldi tmp,0x001
        mov Lnow,tmp        ;schreibe eine 1 in Lnow
        rjmp vergleicheL
weissL:
        ldi tmp,0x000
        mov Lnow,tmp       
vergleicheL:
        cp Lnow,Lold        ;Vergleche zustand alt und neu
        breq auslesenR        ;wenn gleich weiter mit rechtem Rad
        inc zaehlerodo        ;zaehlerodo ++
auslesenR:
        mov Rold,Rnow        ;sichere alten Zustand
        ;lese rechts in tmp
        cbi ADMUX,MUX0
        cli
        sbi ADCSRA,ADSC
AD_BUSYR:
        sbis ADCSR,ADIF
        rjmp AD_BUSYR
       
        in tmp,ADCL
        in tmp,ADCH
        sei
       
        cpi tmp,0xA0 ;vergleiche Ergebnis mit 0xA0
        brlo schwarzR        ;wenn kleiner springe nach schwarz
        rjmp weissR
schwarzR:
        ldi tmp,0x001
        mov Rnow,tmp        ;schreibe eine 1 in Lnow
        rjmp vergleicheR
weissR:
        ldi tmp,0x000
        mov Rnow,tmp       
vergleicheR:
        cp Rnow,Rold        ;Vergleche zustand alt und neu
                breq setzemotoren        ;wenn gleich weiter mit Motorensetzen
        dec zaehlerodo        ;zaehlerodo --
setzemotoren:
        cpi zaehlerodo,0x00
        brlo linksschnell ;vergleiche mit null wenn kleiner dann spring
        dec geschlinks        ;rechts--
        rcall motorengesch
        rjmp loop               
linksschnell:
        inc geschlinks        ;links++
        rcall motorengesch
        rjmp loop

.include "motoren.asm"               
.include "LED.asm"       


;Interrupthandling bei timer 0 überlauf
timer:
        in tmpi,SREG
        push tmpi
        dec zaehler        ;zähler --
        ldi tmpi,anfang        ;Starte Timer neu
        out TCNT0,tmpi
        pop tmpi
        out SREG,tmpi
        reti        ;raus aus dem interrupt
                       
;Interrupthandling bei einer Kolision
kolision:
        in tmpi,SREG
        push tmpi
        ldi statusmot,0x80
        pop tmpi
        out SREG,tmpi
        reti

---

Wenn etwas doch unklar ist, dann einfach fragen

Gruß Sebastian

Hi Sebastian,

wenn ich das richtig entziffere (und Assembler ist für mich eingentlich Write-Only-Code), dann vergleichst du die eingelesenen Odometrie-Werte mit einer festen Schwelle und entscheidest dich für Schwarz oder Weiß. Ändert sich der wert von Schwarz nach Weiß oder umgekehrt, wird - je nach Rad - ein Zähler ehöht oder erniedrigt.
Dabei sehe ich schon das erste Problem: Damit die Odometrie vernünftig funktioniert, muss man eine kleine Hysterese einbauen, sonst kann es passieren, dass ein Übergang doppelt gezählt wird.

Was mir nicht klar ist, ist der Vergleich kleiner Null in "setzemotoren". Der cpi macht doch keinen Vergleich mit Vorzeichen. Damit wird "linksschnell" nie aufgerufen, oder?

CU, Robin

Hallo Archi,
Danke für die Antwort
Die Funktion ist genauso wie Du es beschrieben hast,
Findet ein wechsel der Farbe an einem Rad statt wird ein Zähler um 1 erhöht und an dem anderem um 1 erniedrigt je nach dem od der Zählerstand negativ oder positiv ist sollte sich ein Rad schneller oder langsamer drehen...
Dieses Prinzip habe ich mir auch nicht ausgedacht, sondern hier im Forum gefunden :D
Sollte für den Anfang reichen...

Was die Sache mit cpi gegen null angeht ist das so daß, wenn ich gegen einen negativen Wert vergleiche wird Carry und Negativ in Statusregister gesetzt und den Sprung macht der brlo,
das geht zumindest im Simulator...

Was meinst Du mit der Hysterese ?

Ich habe allerdings einen Fehler gefunden, eine der IR-LEDs hatte eine kaltlötstelle wodurch sie beide mehr aus als an waren, peinlich peinlich, das ist schonmal behoben, aber das Programm klappt immer noch nicht, ich glaube das schreib ich nochmal neu und zwar vergleiche ich alle 10 Bits des AD Wandlers und nicht nur die oberen 8 ....
Wenn ich nur wüßte was Du mit Hysterese meinst würd ich das auch noch ausprobieren
:-k
Danke nochmals Sebastian

Hallo,

Hysterese ist das Verhalten eines z.B. Wechselschalters beim Herd oder Bügeleisen. Er schaltet das Bügeleisen aus, wenn es z.B. über 150 C heiss ist. Es aber erst wieder ein, wenn es unter 120C heiss ist.

Bei der Odometrie heißt das etwa so:
Ist gemessener Wert > 150 und Farbe ist Schwarz dann Farbe jetzt Weiß.
Ist gemessener Wert < 120 und Farbe ist Weiß, dann Farbe jetzt Schwarz.
Damit erreichst du, das ein kleines Zittern des Rades nicht deine Odometriewerte durcheinander bringt.

Oder: http://de.wikipedia.org/wiki/Hysterese

@MSSputnik

Danke für die Antwort,
Habe es inzwischen selber rausbekommen,
Habe einfach nur zu kompliziert gedacht
Gruß Sebastian

Hallo Leute,
sollte es jemanden interessieren, (vielleicht gibt es noch jemanden, der den Asuro in Assembler progt) habe ich jetzt eine Lösung für die Odometrieabfrage gefunden.
Na ja, die Routine habe ich einfach Wejas erweiterten Bibliothek entnommen (danke dafür) und in Assembler umgesetzt
Ich habe versucht den Code gut zu dokumentieren, damit man in etwa was verstehen kann.
Es wird jedes Rad für sich gezählt und zwar in einem 16 Bit Zähler.
Vielleicht gibt es jemanden, der damit was anfangen oder was verbessern kann (bin selber noch nicht lange mit Assembler dran)
Ach ja Hysterese habe ich auch benutzt O:)
Gruß Sebastian

Code:

---

.include "m8def.inc"
;einzelne Bits von toogleregister zuweisen
.equ toogleflagL = 2        ;Hat eine Änderung am Rad Links stattgefunden ?
.equ toogleflagR = 1        ;Hat eine Änderung am Rad Rechts stattgefunden?
.equ toogle = 0                ;Bit 0 von tooglereg für Rad R "0" und Rad L "1" umzuschalten

;Register R1 bis R4 erhalten die Werte der Zähler
.def encoder_leftL = R1
.def encoder_leftH = R2
.def encoder_rightL = R3
.def encoder_rightH = R4
.def tmpi = R16 ; Universalregister für Interrupts
.def tmpi2 = R17 ;Universalregister für 16 Bit rechnen
.def tooglereg = R18        ;Toogle Register, hier werden Flags zu Farbe- u. Radwechsel abgespeichert

.org 0x00
        rjmp reset                ;ResetVector
.org ADCCaddr
        rjmp ADCcomplete        ; ADCcomplete Interrupt Vector
       
;Anfangsfeierlichkeiten sollten mindestens folgendes enthalten

reset:
        ;Stack einrichten
        ldi tmp,HIGH(RAMEND)
        out SPH,tmp
        ldi tmp,LOW(RAMEND)
        out SPL,tmp
        ;Stack fertig
        ;A/D Conversion
        ldi tmp,(1<< ADEN) | (1<<ADFR) | (1<<ADIE) | (1<<ADSC) | (1<<ADPS0) | (1<<ADPS1) | (1<<ADPS2)
        out ADCSRA,tmp
        ldi tmp,(1<<REFS0) | (1<<ADLAR) | (1<<MUX0)
        out ADMUX,tmp
        ;A/D Conversion fertig
        sbi PORTD,PD7 ;schalte Odometrie LEDs ein
       
main:
        ;Hier sollte auch mal ein Programm stehen
        ;Ich lasse es einfach mal weg, damit das alles nicht zu groß wird
        rjmp main 

ADCcomplete:
        in tmpi,SREG                        ;lese Statusregister in tmpi ein
        push tmpi                        ;Rette ihn auf den Stack
        ldi tmpi2,0x00                        ;lade 0x00 in tmpi2(wird benutzt um 16 Bitencoder zu adieren)
        in tmpi,ADCL                        ;lese LOWbit ein(wird nicht benutzt)
        in tmpi,ADCH                        ;Lese HIGHbit ein
        sbrs tooglereg,toogle                ;Wenn Bit toogle = 1 Weiter bei Rechtem Rad
        rjmp radlinks                        ;wenn nicht dann halt bei Links
        ;Rad Rechts
        sbrs tooglereg,toogleflagR        ;ist toogleflagR gesetzt? wenn ja springe zu flagRtrue
        rjmp flagRfalse                        ;wenn nicht springe zu flagRfalse
        ;flagRtrue
        cpi tmpi,0x8C        ;vergleiche mit 0x8C
        brsh ausR        ;wenn größer oder gleich springe zu ausR
        ldi tmpi,0x01        ;lade 0x01 in tmpi
        add encoder_rightL,tmpi;encoder_right++
        adc encoder_rightH,tmpi2;encoder_right++ mit Carry
                        ;debug
                        ;cbi PORTD,PD2        ;Status LED rot an
                        ;sbi PORTB,PB0 ;Status LED grün an
                        ;ende debug
        andi tooglereg,0xFD        ;Lösche toogleflagR
        rjmp ausR        ;Springe zu ausR
flagRfalse:
        cpi tmpi,0xA0        ;vergleiche mit 0xA0
        brlo ausR        ;wenn kleiner springe zu ausR
        ldi tmpi,0x01        ;lade 0x01 in tmpi
        add encoder_rightL,tmpi        ;encoder_right++
        adc encoder_rightH,tmpi2;encoder_right++ mit Carry
                        ;debug
                        ;cbi PORTD,PD2 StatusLED red aus
                        ;sbi PORTB,PB0 StatusLED grün an
                        ;ende debug
        ori tooglereg,(1<<toogleflagR) ;Setze toogleflagR
ausR:
        sbi ADMUX,MUX0        ;schalte Multiplexer auf linkes Rad um
        andi tooglereg,0xFE        ;lösche toogleflag
        rjmp rausadc                ;verlasse den Interrupt
radlinks:
        sbrs tooglereg,toogleflagL        ;ist toogleflagL gesetzt? wenn ja springe zu flagtrue
        rjmp flagLfalse                        ;wenn nicht springe zu flagLfalse
        ;flagLtrue
        cpi tmpi,0x8C                        ;vergleiche mit 0x8C
        brsh ausL                        ;wenn größer oder gleich springe zu ausL
        ldi tmpi,0x01
        add encoder_leftL,tmpi                ;encoder_left++
        adc encoder_leftH,tmpi2                ;encoder_left++ mit Carry
                        ;Debug
                        ;sbi PORTD,PD2 Status LED rot an
                        ;cbi PORTB,PB0 Status LED grün aus
                        ;Ende Debug
        andi tooglereg,0xFB                ;Lösche toogleflagL
        rjmp ausL                        ;verlasse Auswertung Rad Links
flagLfalse:
        cpi tmpi,0xA0        ;vergleiche mit 0xA0
        brlo ausL        ;wenn kleiner springe zu ausL
        ldi tmpi,0x01        ;lade 0x01 in tmpi
        add encoder_leftL,tmpi        ;encoder_left++
        adc encoder_leftH,tmpi2        ;encoder_left++ mit Carry
                        ;Debug
                        ;sbi PORTD,PD2 Status LED rot an
                        ;cbi PORTB,PB0 Status LED grün aus
                        ;ende Debug
        ori tooglereg,(1<<toogleflagL)        ;setze toogleflagL
ausL:       
        cbi ADMUX,MUX0        ;schalte Multiplexer auf Rechtes Rad um
        ori tooglereg,(1<<toogle) ;setze toogleflag
               
rausadc:
        pop tmpi        ;Hole Wert von Stack
        out SREG,tmpi        ;schreibe alten Inhalt in den Statusregister
        reti                ;und tschüß

---