Servocontroller mit Mega16
Salut,
momentan bin ich dabei, fuer ein Hexapod-Projekt einen Controller fuer 16 Servos mit einem Mega16 zu entwickeln. Programm steht soweit, C-Anteil funktioniert auch bereits; nur der Assemblerteil macht Schwierigkeiten. Hier erst einmal der Kerncode, vorzustellen in einem .S-File mit Funktionsdefinitionen und Registersicherung, abgelegt in einer Timer-ISR:
Code:
; clear masks - we do not want to write anything yet
clr amask
clr bmask
clr cmask
ser counter ; count down from 255
sec ; c is to be rotated into cmask
clt ; t is final break condition - cleared
ldi ZH,4 ; RAM pointer is at 1024, address of first servo value
; duration of loop: 32512 cycles - 1.6128 ms of variable pulse width
cycle:
clr ZL ; point Z to first servo
; loop to set lower 8 servos
reg1: rol cmask ; rotate mask to indicate position of current servo in PORTA
brcs end1 ; after 9 rotations: all servos checked, exit loop
ld servo,z+ ; copy value of current servo from RAM and move Z to next value
cp servo,counter
brne reg1 ; if servo is not to be set at current cycle: loop
or amask,cmask ; else add current servo to output mask as high
rjmp reg1 ; and loop
end1:
out _SFR_IO_ADDR(PORTA),amask ; output mask set in loop gets transferred to PORTA
; loop to set upper 8 servos - equivalent to lower 8
reg2: rol cmask
brcs end2
ld servo,z+
cp servo,counter
brne reg2
or bmask,cmask
rjmp reg2
end2:
out _SFR_IO_ADDR(PORTD),bmask ; output mask set in loop to PORTD
brts end ; if final exit condition met: jump to end
dec counter ; count comparision value for servos down
brne cycle ; if not zero: next cycle
set ; if zero: set exit condition
rjmp cycle ; and repeat one last time
end:
Hoffe, das ist so einigermassen verstaendlich. Kerngedanke ist, dass ein Zaehler von 255 runtergeht und 16 sequentiell im SRAM abgelegte Servopositionen mit ihm verglichen werden; stimmt ein Wert ueberein, wird der Servo aktiviert. Je hoeher der Wert ist, desto frueher wird also der Servo zugeschaltet, wobei alle zur gleichen Zeit wieder abschalten (nach der ISR). Naeheres im Codekommentar.
Es geht mir nun darum, den erzeugten Puls zwischen 0 und 1,6128 ms zu regeln (errechnet aus Zyklenzahl); momentan gibts aber 2 Probleme:
1. Liegt ein anderer Wert als 255 an der Registeraddresse, so wird sozusagen eine Reihe unterbrochen, die
2. kumulativ fuer jeden hintereinander auf 255 liegenden Servo eine exponentiell mit der Position steigende Zeit addiert.
Die Pulslaenge kann also nur durch o. g. Anzahl reguliert werden, nicht durch den Wert im SRAM; zudem ist sie fuer alle Servos gleich.
Ich habe keine Ahnung, wo der Fehler liegen koennte - kann mir da vielleicht jemand weiterhelfen?
Gruss und Dank,
David
PS: Falls jemand eine Optimierungsmoeglichkeit entdeckt - ich bin aufgrund der zeitkritischen Aufgabe um jeden Zyklus dankbar! Je weniger Zeit _noetig_ ist, desto besser laesst sich die Sache auf den Servo anpassen (nop)!
[Edit: Kleineren Fehler in der Schleife beseitigt, aendert nichts, Makulatur...]
