AVR: 2 Servos ansteuern (ASM)

[wizzard]

Mit diesem Code steuer ich zwei Servos an, ohne das sie zucken oder ähnliches. Evtl. müssen Min. und Max. Timings für Servos angepasst werden, bei mir funktioniert er bei allen in dieser Form. Ich würde mich sehr über Kommentare freuen (und verbesserungen!)

PS: Sorry wegen den merkwürdigen Einrückungen, aber ich kann keine Dateien anhängen :(
In diesem Forum sollten *.txt *.cpp *.asm etc erlaubt sein!



.include "m8def.inc" ; Definitionen für Atmel ATmega8

;################################################# ############################;

.def pos1 = r20
.def pos2 = r21
.def stat = r22
.def temp = r24
.def tmp2 = r25

;################################################# ############################;

.org 0x000
rjmp INIT ; Reset Handler

.org OVF1addr
rjmp Timer1 ; Timer1 Overflow Handler

;################################################# ############################;

.equ MinValue = 38 ; Der Minimalwert für die linke Position
; Der Wert hängt vom verwendeten Servo ab!
.equ PosServo1 = 50 ; Position von Servo1; 1 <= n <= 100
.equ PosServo2 = 100 ; Position von Servo2; 1 <= n <= 100
; Der Maximalwert hängt auch vom Servo ab

;################################################# ############################;

INIT:

ldi temp, LOW(RAMEND)
out SPL, temp
ldi temp, HIGH(RAMEND)
out SPH, temp

ldi temp, 0xFF
out DDRB, temp ; Port B als Ausgang
ldi temp, 0x00
out PORTB, temp

ldi stat, 0x01 ; Status auf Pos. 1 setzen

ldi pos1, PosServo1 ; Servo1 positionieren
ldi pos2, PosServo2 ; Servo2 positionieren

ldi temp, 0b00000011 ; Prescaler auf 64
out TCCR1B, temp
ldi temp, -0x02 ; Timer1 High auf 0x0271 einstellen (10ms bei 4MHz)
out TCNT1H, temp
ldi temp, -0x71 ; Timer1 Low auf 0x0271 einstellen (10ms bei 4MHz)
out TCNT1L, temp
ldi temp, 0b00000100 ; Timer1 Interrupt initialisieren
out TIMSK, temp

sei ; Interrupts aktivieren

rjmp MAIN

;################################################# ############################;

MAIN:

rjmp MAIN ; Main-Loop, wird später eingebaut

;################################################# ############################;

Timer1: ; Interrupt-Handler
push temp ; temp sichern
push tmp2 ; tmp2 sichern

sbrc stat, 0 ; Falls Status = 1 (Servo1 low)
rcall S1_LO ; Servo1_Low aufrufen

sbrc stat, 1 ; Falls Status = 2 (Servo1 high)
rcall S1_HI ; Servo1_High aufrufen

sbrc stat, 2 ; Falls Status = 3 (Servo2 low)
rcall S2_LO ; Servo2_Low aufrufen

sbrc stat, 3 ; Falls Status = 4 (Servo2 high)
rcall S2_HI ; Servo2_High aufrufen

lsl stat ; Status einen weiter

sbrc stat, 4 ; Wenn Status > 4
ldi stat, 0x01 ; Status wieder bei 1

pop tmp2 ; tmp2 zurück laden
pop temp ; temp zurück laden
reti ; zurück

;################################################# ############################;

S1_LO:
ldi tmp2, 0x00 ; Timer1 High = 0x00
ldi temp, MinValue ; Timer1 Low = Minimum für linken Anschlag
add temp, pos1 ; Dazu Position addieren (0...100)

com tmp2 ; Negieren, da der Timer den Wert nicht
com temp ; herunter-, sondern herauf zählt (bis 0xFF)

out TCNT1H, tmp2 ; Timer1 High setzen
out TCNT1L, temp ; Timer1 Low setzen

sbi PORTB, 0 ; Servo1 High setzen

ret

;################################################# ############################;

S1_HI:
ldi tmp2, 0x02 ; Timer1 High = 0x0271
ldi temp, 0x71 ; Timer1 Low = 0x0271
sbiw tmp2:temp, MinValue ; Zeit f. linken Anschlag abziehen
sub temp, pos1 ; Pos1 Low abziehen
sbci tmp2, 0x00 ; Pos1 High abziehen / Carry anwenden

com tmp2 ; Negieren, da der Timer den Wert nicht
com temp ; herunter-, sondern herauf zählt (bis 0xFF)

out TCNT1H, tmp2 ; Timer1 High setzen
out TCNT1L, temp ; Timer1 Low setzen

cbi PORTB, 0 ; Servo1 Low setzen

ret

;################################################# ############################;

S2_LO:
ldi tmp2, 0x00 ; Timer1 High = 0x00
ldi temp, MinValue ; Timer1 Low = Minimum für linken Anschlag
add temp, pos2 ; Dazu Position addieren (0...100)

com tmp2 ; Negieren, da der Timer den Wert nicht
com temp ; herunter-, sondern herauf zählt (bis 0xFF)

out TCNT1H, tmp2 ; Timer1 High setzen
out TCNT1L, temp ; Timer1 Low setzen

sbi PORTB, 1 ; Servo2 High setzen

ret

;################################################# ############################;

S2_HI:
ldi tmp2, 0x02 ; Timer1 High = 0x0271
ldi temp, 0x71 ; Timer1 Low = 0x0271
sbiw tmp2:temp, MinValue ; Zeit f. linken Anschlag abziehen
sub temp, pos2 ; Pos1 Low abziehen
sbci tmp2, 0x00 ; Pos1 High abziehen / Carry anwenden

com tmp2 ; Negieren, da der Timer den Wert nicht
com temp ; herunter-, sondern herauf zählt (bis 0xFF)

out TCNT1H, tmp2 ; Timer1 High setzen
out TCNT1L, temp ; Timer1 Low setzen

cbi PORTB, 1 ; Servo2 Low setzen

ret

;################################################# ############################;