Code:
; #
; # Testroutine für 2 Servos
; #
; # Controller: ATmega 32
; # Takt: 1 MHz
; #
; # Methode: - PWM über 16 Bit Timer (Timer 1)
; # - Timermode: 14 (siehe Datenblatt Seite 109)
; #
; # Pinbelegung: PD4 - Ausgang - Servo 2
; # PD5 - Ausgang - Servo 1
; #
; # Servoregelbereich: 1001 Einzelschritte (kA ob das nen Servo unterstützt ...)
; # im Detail 999 - 1999 im nachfolgendem Programm (1ms - 2 ms)
; #
;
; Definitionsfile
.include "m32def.inc"
;
; Registeraliase
.undef XL ; hier unnötig und anderweitig genutzt
.undef XH
.def temp = r16
.def servo_1_l = r24 ; Stellung Servo 1
.def servo_1_h = r25
.def servo_2_l = r26 ; Stellung Servo 2
.def servo_2_h = r27
.cseg
.org 0x00
;
; ISR Vektoren
jmp isr_reset ; Reset
.org 0x2A
;
; HW Initialisierung
isr_reset:
ldi temp, high(RAMEND) ; Stackpointer
out SPH, temp
ldi temp, low(RAMEND)
out SPL, temp
ldi temp, 0xFF ; alle ungenutzten Ports -> Eingang + Pullup
out PORTA, temp
out PORTB, temp
out PORTC, temp
ldi temp, (1<<DDD4) | (1<<DDD5) ; Port D4 & D5 -> Ausgang, Rest -> Eingang + PU
out DDRD, temp
ldi temp, (1<<PD0) | (1<<PD1) | (1<<PD2) | (1<<PD3) | (1<<PD6) | (1<<PD7)
out PORTD, temp
ldi temp, high(1499) ; Timer 1 Setup
out OCR1AH, temp
ldi temp, low(1499)
out OCR1AL, temp
ldi temp, high(1499)
out OCR1BH, temp
ldi temp, low(1499)
out OCR1BL, temp
ldi temp, high(19999)
out ICR1H, temp
ldi temp, low(19999)
out ICR1L, temp
ldi temp, (1<<COM1A1) | (1<<COM1B1) | (1<<WGM11)
out TCCR1A, temp
ldi temp, (1<<WGM13) | (1<<WGM12) | (1<<CS10)
out TCCR1B, temp
;
; Main Loop
loop:
ldi servo_1_h, high(1999) ; Servo 1 - ganz nach rechts
ldi servo_1_l, low(1999)
ldi servo_2_h, high(999) ; Servo 2 - ganz nach links
ldi servo_2_l, low(999)
rcall set_servos ; Werte übergeben
rcall warten_1s
ldi servo_1_h, high(1499) ; Servo 1 - Mittelstellung
ldi servo_1_l, low(1499)
ldi servo_2_h, high(1499) ; Servo 2 - Mittelstellung
ldi servo_2_l, low(1499)
rcall set_servos ; Werte übergeben
rcall warten_1s
ldi servo_1_h, high(999) ; Servo 1 - ganz nach links
ldi servo_1_l, low(999)
ldi servo_2_h, high(1999) ; Servo 2 - ganz nach rechts
ldi servo_2_l, low(1999)
rcall set_servos ; Werte übergeben
rcall warten_1s
ldi temp, high(1499)
loop_a: ; Servos sanft in Mittelstellung fahren - dauert ca. 25 Sekunden
sbiw servo_2_l, 1
adiw servo_1_l, 1
rcall set_servos
rcall warten_50ms
cpi servo_1_l, low(1499)
cpc servo_1_h, temp
brne loop_a
rcall warten_1s
rcall warten_1s
rcall warten_1s
jmp loop
set_servos:
out OCR1AH, servo_1_h
out OCR1AL, servo_1_l
out OCR1BH, servo_2_h
out OCR1BL, servo_2_l
ret
warten_50ms: ; 50 ms Warteschleife ...
ldi YL, 0xD2
ldi YH, 0x30
sbiw YL, 1
brne PC-1
ret
warten_1s: ; 1 s Warteschleife
ldi YL, 0x3E
ldi YH, 0x0D
ldi ZL, 0x03
subi YL, 1
sbci YH, 0
sbci ZL, 0
brne PC-3
nop
ret
Wie man sieht, ist der Code doch etwas umfangreicher, was wohl auch auf die Testroutinen für die 2
Code:
; #
; # Testroutine für 16 Servos
; #
; # Controller: ATmega 32
; # Takt: 4 MHz
; #
; # Methode: - Software PWM mittels 8 Bit Timer (Timer 0)
; # - Timermode: 2 (siehe Datenblatt Seite 80)
; #
; # Pinbelegung: PA0 - Ausgang - Servo 1
; # PA1 - Ausgang - Servo 2
; # PA2 - Ausgang - Servo 3
; # PA3 - Ausgang - Servo 4
; # PA4 - Ausgang - Servo 5
; # PA5 - Ausgang - Servo 6
; # PA6 - Ausgang - Servo 7
; # PA7 - Ausgang - Servo 8
; #
; # PB0 - Ausgang - Servo 9
; # PB1 - Ausgang - Servo 10
; # PB2 - Ausgang - Servo 11
; # PB3 - Ausgang - Servo 12
; # PB4 - Ausgang - Servo 13
; # PB5 - Ausgang - Servo 14
; # PB6 - Ausgang - Servo 15
; # PB7 - Ausgang - Servo 16
; #
; # Servoregelbereich: 0 - 85 = 85 Schritte (die scheinbare Ungereimtheit liegt am Software PWM Algo ...)
; # Pulsfolgefrequenz: 20399.75 µs
; #
;
; Definitionsfile
.include "m32def.inc"
;
; Registeraliase
.def sregsave = r5
.def null = r6
.def voll = r7
.def servo_1 = r8
.def servo_2 = r9
.def servo_3 = r10
.def servo_4 = r11
.def servo_5 = r12
.def servo_6 = r13
.def servo_7 = r14
.def servo_8 = r15
.def status = r16 ; Statusregister
; 7 =
; 6 =
; 5 =
; 4 =
; 3 =
; 2 =
; 1 = Preload Servo 9 - 16 done
; 0 = Preload Servo 1 - 8 done
.def temp = r17
.def pulswidth = r18
.def cycle = r19
.def output = r20
;
; Speicher reservieren
.dseg
servo_data: .byte 16
.cseg
.org 0x00
;
; ISR Vektoren
jmp isr_reset ; Reset
.org 0x02
reti ; Int 0
.org 0x04
reti ; Int 1
.org 0x06
reti ; int 2
.org 0x08
reti ; T2 Compare
.org 0x0A
reti ; T2 Overflow
.org 0x0C
reti ; T1 Capture
.org 0x0E
reti ; T1 Compare A
.org 0x10
reti ; T1 Compare B
.org 0x12
reti ; T1 Overflow
.org 0x14
jmp isr_t0_co ; T0 Compare
.org 0x16
reti ; T0 Overflow
.org 0x18
reti ; SPI Transfer Complete
.org 0x1A
reti ; USART - RX Complete
.org 0x1C
reti ; USART - UDR Empty
.org 0x1E
reti ; USART - RX Complete
.org 0x20
reti ; ADC Complete
.org 0x22
reti ; EEPROM Ready
.org 0x24
reti ; Analog Comperator
.org 0x26
reti ; TWI
.org 0x28
reti ; Store Program Memory Ready
.org 0x2A
; HW Initialisierung
isr_reset:
ldi temp, high(RAMEND) ; Stackpointer
out SPH, temp
ldi temp, low(RAMEND)
out SPL, temp
ldi temp, 0xFF ; alle ungenutzten Ports -> Eingang + Pullup
out PORTC, temp
out PORTD, temp
out DDRA, temp ; Port A & B -> Ausgang
out DDRB, temp
ldi temp, 47 ; Timer 0 konfigurieren
out OCR0, temp
ldi temp, (1<<WGM01) | (1<<CS00)
out TCCR0, temp
in temp, TIMSK
ori temp, (1<<OCIE0)
out TIMSK, temp
;
; Speicher und Register initialisieren
ldi temp, 42
ldi ZH, high(servo_data)
ldi ZL, low(servo_data)
st Z+, temp
st Z+, temp
st Z+, temp
st Z+, temp
st Z+, temp
st Z+, temp
st Z+, temp
st Z+, temp
st Z+, temp
st Z+, temp
st Z+, temp
st Z+, temp
st Z+, temp
st Z+, temp
st Z+, temp
st Z, temp
clr pulswidth
clr cycle
clr status
clr null
ldi temp, 0xFF
mov voll, temp
out TCNT0, null
clr temp
sei
;
; Mainloop
loop:
ldi temp, 85
sts servo_data, null ; Servo 1 nach links
sts servo_data+1, temp ; Servo 2 nach rechts
sts servo_data+2, null ; Servo 3 nach links
sts servo_data+3, temp ; Servo 4 nach rechts
sts servo_data+4, null ; Servo 5 nach links
sts servo_data+5, temp ; Servo 6 nach rechts
sts servo_data+6, null ; Servo 7 nach links
sts servo_data+7, temp ; Servo 8 nach rechts
sts servo_data+8, null ; Servo 9 nach links
sts servo_data+9, temp ; Servo 10 nach rechts
sts servo_data+10, null ; Servo 11 nach links
sts servo_data+11, temp ; Servo 12 nach rechts
sts servo_data+12, null ; Servo 13 nach links
sts servo_data+12, temp ; Servo 14 nach rechts
sts servo_data+14, null ; Servo 15 nach links
sts servo_data+15, temp ; Servo 16 nach rechts
rcall warten ; ne Weile Warten
sts servo_data, temp ; und nun genau andersrum ....
sts servo_data+1, null
sts servo_data+2, temp
sts servo_data+3, null
sts servo_data+4, temp
sts servo_data+5, null
sts servo_data+6, temp
sts servo_data+7, null
sts servo_data+8, temp
sts servo_data+9, null
sts servo_data+10, temp
sts servo_data+11, null
sts servo_data+12, temp
sts servo_data+13, null
sts servo_data+14, temp
sts servo_data+15, null
rcall warten ; ne Weile Warten
ldi temp, 42 ; und nun alle Servos auf Mittelstellung
sts servo_data, temp
sts servo_data+1, temp
sts servo_data+2, temp
sts servo_data+3, temp
sts servo_data+4, temp
sts servo_data+5, temp
sts servo_data+6, temp
sts servo_data+7, temp
sts servo_data+8, temp
sts servo_data+9, temp
sts servo_data+10, temp
sts servo_data+11, temp
sts servo_data+12, temp
sts servo_data+13, temp
sts servo_data+14, temp
sts servo_data+15, temp
rcall warten ; ne Weile Warten
jmp loop
;
; Subroutinen
warten: ; Warteschleife
ldi XL, 0x3E ; geschätzte Wartezeit aufgrund der
ldi XH, 0x0D ; hohen Interuptlast ca. 5-6 Sekunden
ldi YL, 0x03 ;
subi XL, 1
sbci XH, 0
sbci YL, 0
brne PC-3
nop
ret
;
; ISR Routinen
isr_t0_co:
in sregsave, SREG
ldi ZH, high(isr_t0_oc_table)
ldi ZL, low(isr_t0_oc_table)
add ZL, cycle
adc ZH, null
ijmp
isr_t0_oc_table:
rjmp isr_t0_oc_0
rjmp isr_t0_oc_1
rjmp isr_t0_oc_2
rjmp isr_t0_oc_3
rjmp isr_t0_oc_4
rjmp isr_t0_oc_0
rjmp isr_t0_oc_0
rjmp isr_t0_oc_0
rjmp isr_t0_oc_0
rjmp isr_t0_oc_0
rjmp isr_t0_oc_0
rjmp isr_t0_oc_0
rjmp isr_t0_oc_0
rjmp isr_t0_oc_0
rjmp isr_t0_oc_0
rjmp isr_t0_oc_0
rjmp isr_t0_oc_0
rjmp isr_t0_oc_0
rjmp isr_t0_oc_0
rjmp isr_t0_oc_19
isr_t0_oc_0:
andi status, 0b11111100
inc pulswidth
cpi pulswidth, 85
brne PC+3
clr pulswidth
inc cycle
out SREG, sregsave
reti
isr_t0_oc_1:
sbrc status, 0
rjmp isr_t0_oc_1_done
lds servo_1, servo_data
lds servo_2, servo_data+1
lds servo_3, servo_data+2
lds servo_4, servo_data+3
lds servo_5, servo_data+4
lds servo_6, servo_data+5
lds servo_7, servo_data+6
lds servo_8, servo_data+7
ori status, 0b00000001
isr_t0_oc_1_done:
out PORTA, voll
inc pulswidth
cpi pulswidth, 85
brne PC+3
clr pulswidth
inc cycle
out SREG, sregsave
reti
isr_t0_oc_2:
cp pulswidth, servo_1
ror output
cp pulswidth, servo_2
ror output
cp pulswidth, servo_3
ror output
cp pulswidth, servo_4
ror output
cp pulswidth, servo_5
ror output
cp pulswidth, servo_6
ror output
cp pulswidth, servo_7
ror output
cp pulswidth, servo_8
ror output
inc pulswidth
cpi pulswidth, 85
brne PC+4
clr pulswidth
inc cycle
clr output
out PORTA, output
out SREG, sregsave
reti
isr_t0_oc_3:
sbrc status, 0 ; Preload schon erledigt ?
rjmp isr_t0_oc_3_done ; - Ja
lds servo_1, servo_data+8
lds servo_2, servo_data+9
lds servo_3, servo_data+10
lds servo_4, servo_data+11
lds servo_5, servo_data+12
lds servo_6, servo_data+13
lds servo_7, servo_data+14
lds servo_8, servo_data+15
ori status, 0b00000010
isr_t0_oc_3_done:
out PORTB, voll
inc pulswidth
cpi pulswidth, 85
brne PC+3
clr pulswidth
inc cycle
out SREG, sregsave
reti
isr_t0_oc_4:
cp pulswidth, servo_1
ror output
cp pulswidth, servo_2
ror output
cp pulswidth, servo_3
ror output
cp pulswidth, servo_4
ror output
cp pulswidth, servo_5
ror output
cp pulswidth, servo_6
ror output
cp pulswidth, servo_7
ror output
cp pulswidth, servo_8
ror output
inc pulswidth
cpi pulswidth, 85
brne PC+4
clr pulswidth
inc cycle
clr output
out PORTB, output
out SREG, sregsave
reti
isr_t0_oc_19:
inc pulswidth
cpi pulswidth, 85
brne PC+6
clr pulswidth
inc cycle
cpi cycle, 20
brne PC+2
clr cycle
out SREG, sregsave
reti
Wie man sieht gehe ich in diesem Beispiel aus Gründen der Geschwindigkeit in der ISR Routine recht verschwenderisch mit den vorhanden Resourcen (sprich den Registern) um. auch ist das ganze nicht wirklich nach Codegröße optimiert.
Lesezeichen