Code:
copyright = Dirk O.
comment = Ansteuerung von 10 Servos
libversion = 2.00
date = 17.06.2006
statement = ------------------------------------
statement = Muss alle 20ms in einer Interrupt- !
statement = Service-Routine ausgeführt werden. !
statement = Geeignet für 1-10 MHz Taktfrequenz !
statement = ------------------------------------
[Servo10]
;Anschluss der 10 Servos:
.equ PORTB = $18 ;Portb des AT90S2313, ATtiny2313
.equ PINB0 = 0 ;Servo1
.equ PINB1 = 1 ;Servo2
.equ PINB2 = 2 ;Servo3
.equ PINB3 = 3 ;Servo4
.equ PINB4 = 4 ;Servo5
.equ PINB5 = 5 ;Servo6
.equ PINB6 = 6 ;Servo7
.equ PINB7 = 7 ;Servo8
.equ PORTD = $12 ;Portd des AT90S2313, ATtiny2313
.equ PIND5 = 5 ;Servo9
.equ PIND6 = 6 ;Servo10
;Register für die 10 Servo-Positionen:
.def Pos1 = r16
.def Pos2 = r17
.def Pos3 = r18
.def Pos4 = r19
.def Pos5 = r20
.def Pos6 = r21
.def Pos7 = r22
.def Pos8 = r23
.def Pos9 = r24
.def Pos10 = r25
.def Temp = r26
.def Position = r27
;BASCOM-Variablen:
; Servoleftpos -> BYTE für den linken Anschlag (1..255, 0=256)
; Impuls 0,6ms -> 1/4/8/10 MHz: 14/ 59/119 /149
; Impuls 0,8ms -> 1/4/8/10 MHz: 19/ 79/159 /199
; Impuls 1,0ms -> 1/4/8/10 MHz: 24/ 99/199 /249
; Servorightpos -> BYTE für den rechten Anschlag (0..255)
; Impuls 1,0ms -> 1/4/8/10 MHz: 28/111/182*/227*
; Impuls 1,2ms -> 1/4/8/10 MHz: 33/133/218*/---
; Impuls 1,4ms -> 1/4/8/10 MHz: 39/156/255*/---
; Zu *: Mit eingefügter Verzögerung (s.u.!)
; Servopos(10) -> BYTE-Array für die 10 Servo-Positionen
; (0..Servorightpos)
Servo10: ;Alle 20ms (Timer 50Hz)
;Impuls für alle 10 Servos starten:
; Der Abstand von 2 Zyklen zwischen den sbi-Befehlen orientiert sich am
; notwendigen Abstand der cbi-Befehle beim Beenden der Impulse (s.u.!).
sbi PORTB,PINB0 ;Servo1 high
*lds Pos1,{Servopos} ;2+
sbi PORTB,PINB1 ;Servo2 high
*lds Pos2,{Servopos+1}
sbi PORTB,PINB2 ;Servo3 high
*lds Pos3,{Servopos+2}
sbi PORTB,PINB3 ;Servo4 high
*lds Pos4,{Servopos+3}
sbi PORTB,PINB4 ;Servo5 high
*lds Pos5,{Servopos+4}
sbi PORTB,PINB5 ;Servo6 high
*lds Pos6,{Servopos+5}
sbi PORTB,PINB6 ;Servo7 high
*lds Pos7,{Servopos+6}
sbi PORTB,PINB7 ;Servo8 high
*lds Pos8,{Servopos+7}
sbi PORTD,PIND5 ;Servo9 high
*lds Pos9,{Servopos+8}
sbi PORTD,PIND6 ;Servo10 high
*lds Pos10,{Servopos+9}
;Impulsdauer (0,6..1ms) linker Anschlag:
; Starten und Beenden des Impulses brauchen zusammen 46 (40 + 6) Zyklen
; plus Verzögerung Servoleftpos * 40 Zyklen (10us bei 4 MHz) ergibt eine
; Impulsdauer linker Anschlag von (Servoleft * 10 + 11,5) us bei 4 MHz.
; Servoleftpos kann für eine bestimmte Impulsdauer so berechnet werden:
; Servoleftpos = Impulsdauer (us) * Taktfrequenz (MHz) / 40 - 1,15
*lds Temp,{Servoleftpos} ;+2=40 Linker Anschlag
Delay0:
call Wait37 ;37+
dec Temp ;1
brne Delay0 ;+2=40
nop
;Beenden des Impulses (nach 0..1,4ms) je nach Position:
; Die kompl. Schleife für alle 10 Servos besteht im Mittel aus 36 Zyklen.
; Damit beträgt die max. Auflösung 9us bei 4 MHz. Bei einer Impulsdauer
; von 1ms sind dann z.B. 111 Servo-Positionen (1000 / 9) ansteuerbar.
; Servorightpos kann für eine bestimmte Impulsdauer so berechnet werden:
; 1/ 4 MHz: Servorightpos = Impulsdauer (us) * Taktfrequenz (MHz) / 36
; 8/10 MHz: Servorightpos = Impulsdauer (us) * Taktfrequenz (MHz) / 44
ldi Position,255 ;1+
Nextpos:
inc Position ;1 Nächste Position (0..Servorightpos)
cp Pos1,Position ;1
brne Next2 ;1
cbi PORTB,PINB0 ;+2=6 Servo1 low
Next2:
cp Pos2,Position ;1+
brne Next3 ;2
cbi PORTB,PINB1 ;Servo2 low
Next3:
cp Pos3,Position
brne Next4
cbi PORTB,PINB2 ;Servo3 low
Next4:
cp Pos4,Position
brne Next5
cbi PORTB,PINB3 ;Servo4 low
Next5:
cp Pos5,Position
brne Next6
cbi PORTB,PINB4 ;Servo5 low
Next6:
cp Pos6,Position
brne Next7
cbi PORTB,PINB5 ;Servo6 low
Next7:
cp Pos7,Position
brne Next8
cbi PORTB,PINB6 ;Servo7 low
Next8:
cp Pos8,Position
brne Next9
cbi PORTB,PINB7 ;Servo8 low
Next9:
cp Pos9,Position
brne Next10
cbi PORTD,PIND5 ;Servo9 low
Next10:
cp Pos10,Position
brne Next11
cbi PORTD,PIND6 ;Servo10 low
Next11:
;call Wait8 ;Nur für 8/10 MHz: + 8 Zyklen!
*lds Temp,{Servorightpos} ;Rechter Anschlag
cp Temp,Position ;Anschlag erreicht?
brne Nextpos ;+2=32(40 b. 8/10 MHz) Nein -> weiter
;Impuls sicherheitshalber für alle Servos beenden!
; (Nur nötig für Servopos(n) > Servorightpos!)
ldi Temp,0
out PORTB,Temp ;Servo1..8 low
cbi PORTD,PIND5 ;Servo9 low
cbi PORTD,PIND6 ;Servo10 low
Wait8:
ret
Wait37: ;29 + 4 (call) + 4 (ret) = 37 Zyklen
call Wait8 ;8+
call Wait8 ;8
call Wait8 ;8
nop
nop
nop
nop
nop ;+1=29
ret
[end]
;---------------------------------------------------------------------------
Servo10 muss in einer ISR in 
Zitieren
Lesezeichen