Odometrie für Asuro in Assembler, brauche Hilfe

[izaseba]

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



.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

[Archi]

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

[izaseba]

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

[MSSputnik]

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

[izaseba]

@MSSputnik

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

[izaseba]

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



.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üß