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