Rastern von Bewegungsvektoren

Um was geht es? der Algorithmus die Umsetzung

Diese Seite ist die zweite einer noch lange nicht vollendeten Serie über die Software eines Fräs- und Bohrroboters. Die Ansteuerung einer solchen Maschine ist sehr komplex und gliedert sich in zahllose Einzelprobleme. Hier wird eine Lösung zum vieldimensionalen Rastern von Bewegungskoordinaten mit Hilfe eines Microcontrollers diskutiert.

Um was geht es?

Die Ortsauflösung des Bohr- oder Fräsautomaten wird von mehreren Größen bestimmt. Üblicherweise wird ein Fräsautomat mit Schrittmotoren betrieben, die ihre Kraft auf Spindeln übertragen, welche wiederum die Drehbewegungen über Muttern in Linearbewegungungen umsetzen. Die Anzahl der Schritte pro Millimeter hängt dabei von der den Schritten pro Umdrehung des Schrittmotors und der Steigung der Spindel ab.
Ein Schrittmotor mit 400 Schritten pro Umdrehung im Halbschrittbetrieb und eine Spindel mit einer Steigung von 5mm pro Umdrehung benötigt dabei 80 Schritte für einen Millimeter. Daher ist die kleinste Einheit einer solcherart ausgerüsteten Anlage 1/80 bzw. 0.0125mm. In diese Einheit müssen alle gewünschten Bewegungen umgesetzt werden. Soll der Fräskopf gleichzeitig um 45,6 mm nach Osten und 71,5 mm nach Norden bewegt werden, so müssen diese Daten in diskrete Schrittfolgen in der Auflösung der Anlage umgerechnet werden. Dies ist auf den ersten Blick ganz einfach gelöst: der Faktor 456/715 oder 0,637762238 bestimmt das Verhältnis beider Schritte. Aber was soll der Controller, der die Schrittmotore steuert, mit diesem Faktor anstellen? Zumal Gleitkommaberechnungen immer fehlerbehaftet sind, da die Anzahl der speicherbaren Stellen begrenzt ist - winzigste Fehler können sich dann bei ein paar Tausend Schritten leicht zu störenden 'Unschärfen' aufsummieren.
Eine bessere Lösung verspricht ein Algorithmus, der dem von Bresenham entwickelten Algorithmus zum Rastern von Linien auf dem Computerbildschirm nachempfunden ist. Dieser wird im Folgenden näher vorgestellt.

Der Algorithmus

Inputdaten für den Algorithmus sind jeweils die relativen Weglänge l_i für die Achse i, also die Angabe: 'gehe von der aktuellen Position -64 Schritte in X-Richtung und +36 Schritte in Y-Richtung'. Benötigt wird ein Zähler z_i pro Achse i. Es gibt p Achsen.

m = max(l_i) für i = 1,2,...,p
z_i = m / 2

z_i = z_i + l_i
Wenn z_i >= m mache einen Schritt für Achse i und setze z_i = z_i - m

Wiederhole Schritte 3 und 4 für i = 1,2,...,p

Wiederhole Schritte 3 bis 5 bis zum Erreichen der Zielkoordinaten.

Das hört sich jetzt alles sehr schön an - weises Nicken im Auditorium - aber funktioniert es auch wirklich? Ein kleines Experiment: Es gibt 2 Achsen. Der Fräskopf soll von der rot markierten Startposition aus um 5 Schritte in X-Richtung und 3 in Y-Richtung bewegt werden, um eine Linie zu ziehen.

Rasterung

Aktion	X-Achse	Y-Achse	Schritte
	Strecke = 5	Strecke = 3
Berechne das Maximum der Weglängen	Maximum `m=5`
Initialisieren der Zähler	2	2
Schritt 1:	2	0	X,Y
Schritt 2:	2	3	X
Schritt 3:	2	1	X,Y
Schritt 4:	2	4	X
Schritt 5:	2	2	X,Y

Die Umsetzung

In (Pseudo)C könnte man die pro Schritt für alle Achsen auszuführende Formel folgendermaßen beschreiben:

z_i=((z_i+l_i)>=m)?(z_i+l_i-m):(z_i+l_i)

Was ein einfacher C-Compiler für Microcontroller daraus macht, ist zumeist eine längere Rechnung - ich habe keine gute Meinung von den Optimizern von uC-Compilern. In Assembler kann man diese Formel jedoch auf nur zwei extrem performant berechenbare Anweisungen zuammenschrumpfen:

Berechne z_i = z_i + l_i
Berechne q_i = z_i - m

Die erste Anweisung berechnet einfach die Erhöhung des Zählers. Der Trick liegt in der Subtraktion der zweiten Anweisung: Die Subtraktion ist gleichzeitig Vergleich und Berechnung! Ist der Subtrahend m größer als der Minuend z_i, resultiert die Operation in einem Überlauf und somit einem gesetzten Carry-Flag. In diesem Falle war z_i < m, das Endergebnis ist z_i, und die Achse führt keinen Schritt aus. Ist das Carry-Flag hingegen gelöscht, so war z_i >= m, ein Schritt muss ausgeführt werden, und das Endergebnis ist q_i.

Bei nur einem Byte würde die Anlage bei 80 Schritten je Millimeter nur etwas mehr als 3 mm verfahren können. Aus diesem Grunde ist eine Erweiterung auf 2 oder 3 Byte angeraten. Um ein paar Takte Rechenzeit zu sparen, kann man auf die Berechnung der 'Leitachse', also der Quelle des maximalen Streckenwertes, verzichten: diese Achse führt ohnehin bei jedem Anlagentakt einen Schritt aus. Desweiteren ist noch ein Schrittzähler für die Leitachse vorzusehen, der das Ende des Weges erkennen muss.

Die Hardware ist folgendermaßen angebunden: Es gibt für jede Achse eine Leitung 'Clock' (XCK, YCK, ZCK). Bei jeder Low-to-High Transition auf dieser Leitung bewegt sich der Schrittmotor dieser Achse um 1/80 mm in die Richtung, die von der Leitung 'Richtung' (XHD, YHD, ZHD) bestimmt wird. Als dritte Leitung gibt es noch 'Enable' (XEN, YEN, ZEN), womit der Strom der Achse ein- und ausgeschaltet werden kann. Dieses Interface wird hardwareseitig von L297/L298-Chips ausgewertet.

Dieses Beispielprogramm ist ein Teil meines aktuellen Fräs- und Bohrroboterprogramms für eine Anlage mit 3 Achsen. Ich habe mich zwar bemüht, alles unwesentliche herauszufiltern, wie beispielsweise die Berechnung des Beschleunigungs- und Bremsvorganges oder die Datenladeroutine, aber fast 500 Zeilen sind dennoch übriggeblieben.

Der Start des Programms beginnt bei dem Label 'start:', bei dem alle wichtigen internen Register initialisiert werden. Beim Label 'main:' werden in diversen Unterprogrammaufrufen die Leitachse festgelegt, der Maximalwert ermittelt, die Steuerausgänge für die Schrittmotore gesetzt und anschließend der Timer 0 eingeschaltet. Von nun an führt dieser Timer die Schritte für die einzelnen Achsen nach Bedarf aus und zählt einen Schrittzähler so lange herunter, bis dieser 0 erreicht. Abschließend wird der Strom für alle Achsen ausgeschaltet und das Programm kontrolliert beendet.


; Steuerung der Hardware
	XCK		EQU	P3.7	; Clock
	YCK		EQU	P1.2	; rising edge
	ZCK		EQU	P1.5

	XHD		EQU	P1.0	; Richtung
	YHD		EQU	P1.3
	ZHD		EQU	P1.6

	XEN		EQU	P1.1	; Enable
	YEN		EQU	P1.4	; active High
	ZEN		EQU	P1.7

	XSTEPH		DATA	009h	; Schritte in X-Richtung high
	XSTEPM		DATA	00Ah	; Schritte in X-Richtung middle
	XSTEPL		DATA	00Bh	; Schritte in X-Richtung low
	YSTEPH		DATA	00Ch	; Schritte in Y-Richtung
	YSTEPM		DATA	00Dh
	YSTEPL		DATA	00Eh
	ZSTEPH		DATA	00Fh	; Schritte in Z-Richtung
	ZSTEPM		DATA	010h
	ZSTEPL		DATA	011h

	MAXSTEPH	DATA	012h	; die größte Schrittanzahl
	MAXSTEPM	DATA	013h	; die größte Schrittanzahl
	MAXSTEPL	DATA	014h	; die größte Schrittanzahl

	XBRESH		DATA	015h	; Bresenham X-Richtung high
	XBRESM		DATA	016h	; Bresenham X-Richtung middle
	XBRESL		DATA	017h	; Bresenham X-Richtung low
	YBRESH		DATA	018h	; Bresenham Y-Richtung
	YBRESM		DATA	019h
	YBRESL		DATA	01Ah
	ZBRESH		DATA	01Bh	; Bresenham Z-Richtung
	ZBRESM		DATA	01Ch
	ZBRESL		DATA	01Dh

	XHEAD		BIT	000h	; Richtung, Byte 20h
	YHEAD		BIT	001h
	ZHEAD		BIT	002h
	STOPFLAG	BIT	003h	; Flag zum Stoppen 

	XISLEADING	BIT	003h	; Wenn 1 dann Leitachse
	YISLEADING	BIT	004h
	ZISLEADING	BIT	005h

	TH0BUF 		EQU	0F2h	; Geschwindigkeit
	TL0BUF 		EQU	053h

org 0h
        ajmp start
;---------------------------------------------------------------;
; hier die Interruptroutinen eintragen 				;
;---------------------------------------------------------------;

org 0Bh				; Timer 0

	mov TH0,#TH0BUF
	mov TL0,#TL0BUF

; X-Achse
doXAxis:
				; Leitachse?
	jb XISLEADING,makeXStepNow

	mov a,XSTEPL		; zu den Entscheidungsvariablen 
	add a,XBRESL		; die Schrittzahl addieren
	mov XBRESL,a
	mov a,XSTEPM
	addc a,XBRESM
	mov XBRESM,a
	mov a,XSTEPH
	addc a,XBRESH
	mov XBRESH,a

	clr c			; Ergebnis größer der größten
	mov a,XBRESL		; Schrittweite?
	subb a,MAXSTEPL	
	mov R2,a
	mov a,XBRESM	
	subb a,MAXSTEPM	
	mov R1,a
	mov a,XBRESH	
	subb a,MAXSTEPH
	mov R0,a

	jnc makeXStep
	jmp doYAxis

makeXStep:
	mov XBRESH,R0		; Ergebniswerte abspeichern
	mov XBRESM,R1
	mov XBRESL,R2	

makeXStepNow:
	clr XCK			; Schritt ausführen
	nop
	nop
	nop
	nop
	setb XCK		; Clock wieder high setzen

; Y-Achse
doYAxis:
				; Leitachse?
	jb YISLEADING,makeYStepNow

	mov a,YSTEPL		; zu den Entscheidungsvariablen 
	add a,YBRESL		; die Schrittzahl addieren
	mov YBRESL,a
	mov a,YSTEPM
	addc a,YBRESM
	mov YBRESM,a
	mov a,YSTEPH
	addc a,YBRESH
	mov YBRESH,a

	clr c			; Ergebnis größer der größten
	mov a,YBRESL		; Schrittweite?
	subb a,MAXSTEPL	
	mov R2,a
	mov a,YBRESM	
	subb a,MAXSTEPM	
	mov R1,a
	mov a,YBRESH	
	subb a,MAXSTEPH
	mov R0,a

	jnc makeYStep
	jmp doZAxis

makeYStep:
	mov YBRESH,R0		; Ergebniswerte abspeichern
	mov YBRESM,R1
	mov YBRESL,R2	

makeYStepNow:
	clr YCK			; Schritt ausführen
	nop
	nop
	nop
	nop
	setb YCK		; Clock wieder high setzen

; Z-Achse
doZAxis:
				; Leitachse?
	jb ZISLEADING,makeZStepNow

	mov a,ZSTEPL		; zu den Entscheidungsvariablen 
	add a,ZBRESL		; die Schrittzahl addieren
	mov ZBRESL,a
	mov a,ZSTEPM
	addc a,ZBRESM
	mov ZBRESM,a
	mov a,ZSTEPH
	addc a,ZBRESH
	mov ZBRESH,a

	clr c			; Ergebnis größer der größten
	mov a,ZBRESL		; Schrittweite?
	subb a,MAXSTEPL	
	mov R2,a
	mov a,ZBRESM	
	subb a,MAXSTEPM	
	mov R1,a
	mov a,ZBRESH	
	subb a,MAXSTEPH
	mov R0,a

	jnc makeZStep
	jmp bresenhamCntDn

makeZStep:
	mov ZBRESH,R0		; Ergebniswerte abspeichern
	mov ZBRESM,R1
	mov ZBRESL,R2	

makeZStepNow:
	clr ZCK			; Schritt ausführen
	nop
	nop
	nop
	nop
	setb ZCK		; Clock wieder high setzen

bresenhamCntDn:
	setb c			; Schrittzähler runterzählen
	mov a,CNTDNL
	subb a,#0
	mov CNTDNL,a
	mov a,CNTDNM
	subb a,#0
	mov CNTDNM,a
	mov a,CNTDNH
	subb a,#0
	mov CNTDNH,a
	jc bresenhamEnde

	reti

;- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
bresenhamEnde:			; Schleife fertig
; alles dichtmachen
	mov TCON,#0	; Timer abstellen
	mov IE,#0	; Interrupts abstellen
	
	clr XEN
	clr YEN
	clr ZEN
	setb XCK
	setb YCK
	setb ZCK

	setb STOPFLAG
	reti

;---------------------------------------------------------------;
; hier die Funktionen eintragen				        ;
;---------------------------------------------------------------;
; 24 Bit-Subtraktion/Vergleich, A=A-B
; Input:	A	R0	high
; 			R1
; 			R2	low
; 		B	R4	high
; 			R5
; 			R6	low
F_subb24:
	clr c

	mov a,R2	; 1. Subtraktion
	subb a,R6	
	mov R2,a

	mov a,R1	; 2. Subtraktion
	subb a,R5
	mov R1,a

	mov a,R0	; 3. Subtraktion
	subb a,R4
	mov R0,a

	ret
;- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
getNextParameter:
; hier nur statisch mit einem Beispiel vorbelegt
; X-Achse
	mov XHEAD,#1
	mov XSTEPH,#0
	mov XSTEPM,#10
	mov XSTEPL,#0
; Y-Achse
	mov YHEAD,#0
	mov YSTEPH,#0
	mov YSTEPM,#2
	mov YSTEPL,#0
; Z-Achse
	mov ZHEAD,#0
	mov ZSTEPH,#0
	mov ZSTEPM,#0
	mov ZSTEPL,#255

	ret
;- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
setHeadingAndEnable:
	mov c,XHEAD	; Richtung ausgeben
	mov XHD,c
	mov c,YHEAD
	mov YHD,c
	mov c,ZHEAD
	mov ZHD,c

	setb XEN
	setb YEN
	setb ZEN
	ret
;- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
; die Variablen zur Schrittentscheidung  mit MAXSTEP/2 vorbelegen,
; damit der Schrittkanal in der Mitte der gewünschten Ideallinie liegt
setVariables:
	clr c
	mov a,MAXSTEPH
	rrc a
	mov XBRESH,a
	mov YBRESH,a
	mov ZBRESH,a
	mov RAMPSTOPCNTH,a

	clr c
	mov a,MAXSTEPM
	rrc a
	mov XBRESM,a
	mov YBRESM,a
	mov ZBRESM,a
	mov RAMPSTOPCNTM,a
	
	clr c
	mov a,MAXSTEPL
	rrc a
	mov XBRESL,a
	mov YBRESL,a
	mov ZBRESL,a
	mov RAMPSTOPCNTL,a

; die Countdown-Variablen belegen
	mov CNTDNH,MAXSTEPH
	mov CNTDNM,MAXSTEPM
	mov CNTDNL,MAXSTEPL
	ret
;- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
setMaxValue:
; den größten Schrittwert herausfinden, Leitachse festlegen
	clr XISLEADING
	clr YISLEADING
	clr ZISLEADING

	mov R0,XSTEPH
	mov R1,XSTEPM
	mov R2,XSTEPL
	mov R4,YSTEPH
	mov R5,YSTEPM
	mov R6,YSTEPL
	call F_subb24		; Vergleiche X und Y

	mov R0,ZSTEPH
	mov R1,ZSTEPM
	mov R2,ZSTEPL

	jc YgtX			; Y>X?

	mov R4,XSTEPH		; Vergleiche X und Z
	mov R5,XSTEPM
	mov R6,XSTEPL
	call F_subb24	
	jc Xgreatest		; X>Z?
	jmp Zgreatest

YgtX:
	call F_subb24		; Vergleiche Y und Z
	jc Ygreatest		; Y>Z?
	
Zgreatest:			; Z ist der größte Wert!
	mov MAXSTEPH,ZSTEPH
	mov MAXSTEPM,ZSTEPM
	mov MAXSTEPL,ZSTEPL
	setb ZISLEADING
	jmp bresenham_L0
	
Ygreatest:			; Y ist der größte Wert!
	mov MAXSTEPH,YSTEPH
	mov MAXSTEPM,YSTEPM
	mov MAXSTEPL,YSTEPL
	setb YISLEADING
	jmp bresenham_L0

Xgreatest:			; X ist der größte Wert!
	mov MAXSTEPH,XSTEPH
	mov MAXSTEPM,XSTEPM
	mov MAXSTEPL,XSTEPL
	setb XISLEADING
bresenham_L0:
	ret


; Initialisierung
;---------------------------------------------------------------
start:
	clr XEN
	clr YEN
	clr ZEN
	
	setb XCK
	setb YCK
	setb ZCK

; Timer 0 aktivieren
;  Intervall: 1 ms
;  Software-Kontrolle
        mov TH0, #TH0BUF
        mov TL0, #TL0BUF

; die SFR's initialisieren
        mov SP,  #60h
        mov TMOD,#17

; Hauptprogramm
;---------------------------------------------------------------
main:
	setb XHD
	setb YHD
	setb ZHD
	setb XCK
	setb YCK
	setb ZCK
	clr  XEN
	clr  YEN
	clr  ZEN

bresenham:
	
	call getNextParameter	; Parameter holen

	call setMaxValue
	call setVariables

	call setHeadingAndEnable

	clr STOPFLAG
        mov IE,  #138	; Interrupts anstellen
        mov TCON,#16	; Timer anstellen
        
bresenhamAgain:		; Schleife wartet auf Ende des Vorschubs
	jnb STOPFLAG, bresenhamAgain

	clr XEN		; alles Disablen
	clr YEN
	clr ZEN
	
	mov TCON,#0	; Timer abstellen
	mov IE,#0	; Interrupts abstellen
	
	setb XCK	; auf definierten Wert stellen
	setb YCK
	setb ZCK

alleshalt:
	jmp alleshalt

END

zurück zum Anfang

Erik Buchmann

EMail an: Owner@ErikBuchmann.de