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
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

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