Hier ist der code:
Code:
Main:
rcall Berechnung
rcall Ausgabe
push A1
ldi A1,17
rcall WaitMs
pop A1
rjmp Main
;-----------------------------------------------
; UP zur ansteuerung der Servos
;-----------------------------------------------
Ausgabe:
push A1
push A2
push B1
push B2
clr X1
ser A1
cli
ser B1
out Portd,B1
out Portb,A1
push B1
ldi B1,250
loopp: ldi B2,6
loop: dec B2
brne loop
dec B1
brne loopp
pop B1
Se1:
cp X1,var1
brne wait1
clr B1
subi A1,0b00000001
nop
rjmp Se2
wait1:
nop
nop
nop
Se2:
cp X1,var2
brne wait2
subi A1,0b00000010
nop
rjmp se3
wait2:
nop
nop
nop
Se3:
cp X1,var3
brne wait3
subi A1,0b00000100
nop
rjmp se4
wait3:
nop
nop
nop
Se4:
cp X1,var4
brne wait4
subi A1,0b00001000
nop
rjmp se5
wait4:
nop
nop
nop
Se5:
cp X1,var5
brne wait5
subi A1,0b00010000
nop
rjmp se6
wait5:
nop
nop
nop
Se6:
cp X1,var6
brne wait6
subi A1,0b00100000
nop
rjmp se7
wait6:
nop
nop
nop
Se7:
nop
out Portd,B1
out portb,A1
nop
ser A2
cp X1,A2
brne se8
sei
pop B2
pop B1
pop A2
pop A1
ret
se8:
inc X1
rjmp Se1
;-----------------------------------------------
waitMs: push A2
push B1
push B2
loop1: ldi r17,10
loop2: ldi r18,159
nop
nop
loop3: dec r18
nop
nop
brne loop3
dec r17
brne loop2
dec r16
brne loop1
pop B2
pop B1
pop A2
ret ;Rücksprung
Das Up Berechnung ist nur eine Abfrage des ADC Ports und speichert den Wert in var1 (servo1).
Ich verwende nen Mega168 und die Routine dauert 19,2ms, Kann aber recht schnell runtergedreht werden, werd ich auch mal testen.
edit: habs gad mit ner wait schleife mit 15 ms ausprobiert, will aber auch nicht.
mfg robin
Lesezeichen