AVR-Tutorial: Servo

Wechseln zu: Navigation, Suche

Die Seite ist noch im Entstehen und, bis sie einigermaßen vollständig ist, noch kein Teil des Tutorials

Vorbemerkung[Bearbeiten]

Dieses Tutorial überschneidet sich inhaltlich mit dem Artikel Modellbauservo_Ansteuerung. Wir beschreiben hier die grundsätzliche Vorgehensweise zur Ansteuerung von Servos und geben ein Beispiel in Assembler, während der genannte Artikel ein in C formuliertes Beispiel zur Ansteuerung mehrerer Servos liefert.

Allgemeines über Servos[Bearbeiten]

Stromversorgung[Bearbeiten]

Werden Servos an einem µC betrieben, so ist es am Besten, sie aus einer eigenen Stromquelle (Akku) zu betreiben. Manche Servos erzeugen kleine Störungen auf der Versorgungsspannung, die einen µC durchaus zum Abstürzen bringen können. Muss man Servos gemeinsam mit einem µC von derselben Stromquelle betreiben, so sollte man sich gleich darauf einrichten, diesen Störimpulsen mit Kondensatoren zu Leibe rücken zu müssen. Unter Umständen ist hier auch eine Mischung aus kleinen, schnellen Kondensatoren (100nF) und etwas größeren, aber dafür auch langsameren Kondensatoren (einige µF) notwendig.

Die eindeutig beste Option ist es aber, die Servos strommäßig vom µC zu entkoppeln und ihnen ihre eigene Stromquelle zu geben. Servos sind nicht besonders heikel. Auch im Modellbau müssen sie mit unterschiedlichen Spannungen zurechtkommen, bedingt durch die dort übliche Versorgung aus Akkus, die im Laufe der Betriebszeit des Modells natürlich durch die Entladung ihre Voltzahl immer weiter reduzieren. Im Modellbau werden Akkus mit 4 oder 5 Zellen verwendet, sodass Servos mit Spannungen von ca. 4V bis hinauf zu ca. 6V zurecht kommen müssen, wobei randvolle Akkus diese 6V schon auch mal überschreiten können. Bei sinkenden Spannungslage verlieren Servos naturgemäß etwas an Kraft bzw. werden in ihrer Stellgeschwindigkeit unter Umständen langsamer.

Die Servos werden dann nur mit ihrer Masseleitung und natürlich mit ihrer Impulsleitung mit dem µC verbunden.

Das Servo-Impulstelegram[Bearbeiten]

Das Signal, das an den Servo geschickt wird, hat eine Länge von ungefähr 20ms. Diese 20ms sind nicht besonders kritisch und sind ein Überbleibsel von der Technik mit der mehrere Kanäle über die Funkstrecke einer Fernsteuerung übertragen werden. Für das Servo wichtig ist die Impulsdauer in der ersten Phase eines Servosignals. Nominell ist dieser Impuls zwischen 1ms und 2ms lang. Wobei das jeweils die Endstellungen des Servos sind, an denen es noch nicht mechanisch begrenzt wird. Eine Pulslänge von 1.5ms wäre dann Servomittelstellung. Für die Positionsauswertung des Servos haben die 20ms Wiederholdauer keine besondere Bedeutung, sieht man einmal davon ab, dass ein Servo bei kürzeren Zeiten entsprechend öfter Positionsimpulse bekommt und daher auch öfter die Position gegebenenfalls korrigiert, was möglicherweise in einem etwas höheren Stromverbrauch resultiert.

Umgekehrt lässt sich definitiv Strom sparen, indem die Pulse ganz ausgesetzt werden: Der Servo bleibt in der Position, in der er sich gerade befindet - korrigiert sich aber auch nicht mehr. Kommen die Impulse selten, also z.B. alle 50ms, läuft der Servo langsamer in seine Zielposition (praktische Erfahrungen, vermutlich nirgends spezifiziert). Dieses Verhalten lässt sich nutzen, um die manchmal unerwünschten ruckartigen Bewegungen eines Servos abzumildern.

Servo Impulsdiagramm

Den meisten Servos macht es nichts aus, wenn die Länge des Servoprotokolls anstelle von 20ms auf zb 10ms verkürzt wird. Bei der Generierung des Servosignals muss man daher den 20ms keine besondere Beachtung schenken. Eine kleine Pause nach dem eigentlichen Positionssignal reicht in den meisten Fällen aus und es spielt keine allzugroße Rolle, wie lange diese Pause tatsächlich ist. Generiert man das Imulsdiagramm zb. mit einem Timer, so orientiert man sich daher daran, dass man den 1.0 - 2.0ms Puls gut generieren kann und nicht an den 20ms.

Reale Servos haben allerdings in den Endstellungen noch Reserven, so dass man bei vielen Servos auch Pulslängen von 0.9 bis 2.1 oder sogar noch kleinere/größere Werte benutzen kann. Allerdings sollte man hier etwas Vorsicht walten lassen. Wenn das Servo unbelastet in einer der Endstellungen deutlich zu 'knurren' anfängt, dann hat man es übertrieben. Das Servo ist an seinen mechanischen Endanschlag gefahren worden und auf Dauer wird das der Motor bzw. das Getriebe nicht aushalten.

Programmierung[Bearbeiten]

einfache Servoansteuerung mittels Warteschleifen[Bearbeiten]

Im folgenden Programm wurden einfache Warteschleifen auf die im Tutorial übliche Taktfrequen von 4Mhz angepasst, so dass sich die typischen Servo-Pulsdauern ergeben. Ein am Port D, beliebiger Pin angeschlossenes Servo dreht damit ständig vor und zurück. Die Servoposition kann durch laden eines Wertes im Bereich 1 bis ca 160 in das Register r18 und anschliessendem Aufruf von servoPuls in einen Puls für ein Servo umgewandelt werden.

 
.include "m8def.inc"
 
.equ XTAL = 4000000
 
         rjmp    init

init:
          ldi      r16, HIGH(RAMEND)     ; Stackpointer initialisieren
          out      SPH, r16
          ldi      r16, LOW(RAMEND)
          out      SPL, r16

          ldi r16, 0xFF
          out DDRD, r16

loop:     ldi  r18, 0
           
loop1:    inc r18
          cpi r18, 160
          breq loop2

          rcall servoPuls

          rjmp loop1

loop2:    dec r18
          cpi r18, 0
          breq loop1

          rcall servoPuls

          rjmp loop2

servoPuls:
          push r18
          ldi r16, 0xFF         ; Ausgabepin auf 1
          out PORTD, r16
          
          rcall wait_puls        ; die Wartezeit abwarten

          ldi r16, 0x00         ; Ausgabepin wieder auf 0
          out PORTD, r16

          rcall wait_pause       ; und die Pause hinten nach abwarten
          pop r18
          ret
;
wait_pause:
          ldi r19, 15
w_paus_1: rcall wait_1ms
          dec r19
          brne w_paus_1
          ret
;
wait_1ms: ldi r18, 10           ; 1 Millisekunde warten
w_loop2:  ldi r17, 132          ; Es muessen bei 4 Mhz 4000 Zyklen verbraten werden
w_loop1:  dec r17               ; die innerste Schleife umfasst 3 Takte und wird 132
          brne w_loop1          ; mal abgearbeitet: 132 * 3 = 396 Takte
          dec r18               ; dazu noch 4 Takte für die äussere Schleife = 400
          brne w_loop2          ; 10 Wiederholungen: 4000 Takte
          ret                   ; der ret ist nicht eingerechnet
;
; r18 muss mit der Anzahl der Widerholungen belegt werden
; vernünftige Werte laufen von 1 bis ca 160
wait_puls:
w_loop4:  ldi r17, 10           ; die variable Zeit abwarten
w_loop3:  dec r17
          brne w_loop3
          dec r18
          brne w_loop4

          rcall wait_1ms         ; und noch 1 Millisekunde drauflegen
          ret

Wie meistens gilt auch hier: Warteschleifen sind in der Programmierung nicht erwünscht. Der Prozessor kann in diesen Warteschleifen nichts anderes machen. Etwas ausgeklügeltere Programme, bei denen mehrere Dinge gleichzeitig gemacht werden sollen, sind damit nicht vernünftig realisierbar. Daher sollte die Methode mittels Warteschleifen nur dann benutzt werden, wenn dies nicht benötigt wird, wie zb einem simplen Servotester, bei dem man die Servoposition zb durch Auslesen eines Potis mit dem ADC festlegt.

Ausserdem ist die Berechnung der Warteschleifen auf eine bestimmte Taktfrequenz unangenehm und fehleranfällig :-)

einfache Servoansteuerung mittels Timer[Bearbeiten]

Ansteuerung mit dem 16-Bit Timer 1[Bearbeiten]

Im Prinzip programmiert man sich hier eine Software-PWM. Beginnt der Timer bei 0 zu zählen (nach Overflow Interrupt oder Compare Match Interrupt im CTC-Modus (Clear Timer on Compare)), so setzt man den gewünschten Ausgangspin auf 1. Ein Compare Match Register wird so mit einem berechneten Wert versorgt, daß es nach der gewünschten Pulszeit einen Interrupt auslöst. In der zugehörigen Interrupt Routine wird der Pin dann wieder auf 0 gesetzt.

Auch hier wieder: Der Vorteiler des Timers wird so eingestellt, dass man die Pulszeit gut mit dem Compare Match erreichen kann, die nachfolgende Pause, bis der Timer dann seinen Overflow hat (oder den CTC Clear macht) ist von untergeordneter Bedeutung. Man nimmt was vom Zählbereich des Timers übrig bleibt. Beispiel:

  • F_CPU = 16 MHz
  • Vorteiler = 8
  • 16 Bit Registerbreite = Overflow nach 2^16 Zyklen
  • CTC-Modus *aus*, steigende Flanke durch Overflow-Interrupt erzeugen
  • Overflow-Interrupt: 16 MHz / 2^16 / 8 = 30,52 Hz, 32,8 ms, Ausgangspin auf 1 setzen
  • Servostellung links .. rechts = Pulsbreite 1 .. 2 ms = 1 s / 16000000 * 8 * OCR1; OCR1 = 2000 .. 4000
  • Output-Compare-Match-Interrupt: Ausgangspin auf 0 setzen

Alternativ zum Overflow-Interrupt könnte, um eine Gesamtpulsbreite von exakt 20 ms zu erreichen, z.B. im Output-Compare-Match-Interrupt (sofern nur einer pro Timer verfügbar ist) im Wechsel

  • der Ausgangspin auf 1 und OCR1 auf 1000 .. 2000 gesetzt sowie das CTC-Bit gelöscht werden
  • der Ausgangspin auf 0 und OCR1 auf 40000 gesetzt sowie das CTC-Bit gesetzt werden

Neuere ATmegas verfügen über zwei Output-Compare-Register (Suffix A und B) ebenso wie zwei OCR-Interrupt-Vektoren pro Timer, wobei das CTC-Bit nur bei einem Match mit dem Output-Compare-Register A wirkt. Damit können für die Pulserzeugung zwei getrennte Interrupt-Handler angelegt werden, und ein Umbelegen von OCR und CTC-Bit entfällt.

Bei einem Arduino kann man auch die Pins 9 und 10 direkt per Hardware ganz ohne Interrupts ansteuern:

setup() {
    DDRB |= _BV(DDB1) | _BV(DDB2);                   // set pins OC1A = PortB1 -> PIN 9 and OC1B = PortB2 -> PIN 10 to output direction
    TCCR1A = _BV(COM1A1) | _BV(COM1B1) | _BV(WGM11); // FastPWM Mode mode TOP determined by or ICR1 - non-inverting Compare Output mode
    TCCR1B = _BV(WGM13) | _BV(WGM12) | _BV(CS11);    // set prescaler to 8, FastPWM Mode mode continued
    ICR1 = 40000;      // set period to 20 ms
    OCR1A = 3000;      // set count to 1500 us - 90 degree
    OCR1B = 3000;      // set count to 1500 us - 90 degree
    TCNT1 = 0;         // reset timer
}
void writeServoCounter(int tDegree) {
    uint16_t tCounter = map(tDegree, 0, 180, 1000, 5200); // for SG90 micro Servo
    OCR1A = tCounter;
    Serial.print(tCounter);
    Serial.println("cnt");
}

Ansteuerung mit dem 8-Bit Timer 2[Bearbeiten]

Mit einem 8 Bit Timer ist es gar nicht so einfach, sowohl die Zeiten für den Servopuls als auch die für die Pause danach unter einen Hut zu bringen. Abhilfe schafft ein Trick.

Der Timer wird so eingestellt, dass sich der Servopuls gut erzeugen lässt. Dazu wird der Timer in den CTC Modus gestellt und das zugehörige Vergleichsregister so eingestellt, dass sich die entsprechenden Interrupts zeitlich so ergeben, wie es für einen Puls benötigt wird. In einem Aufruf des Interrupts wird der Ausgangspin für das Servo auf 1 gestellt, im nächsten wird er wieder auf 0 gestellt. Die kleine Pause bis zum nächsten Servoimpuls wird so erzeugt, dass eine gewisse Anzahl an Interrupt Aufrufen einfach nichts gemacht wird. Ähnlich wie bei einer PWM wird also auch hier wieder ein Zähler installiert, der die Anzahl der Interrupt Aufrufe mitzählt und immer wieder auf 0 zurückgestellt wird.

Die eigentliche Servoposition steht im Register OCR2. Rein rechnerisch beträgt ihr Wertebereich:

1ms 4000000 / 64 / 1000 OCR2 = 62.5

2ms 4000000 / 64 / 500 OCR2 = 125

mit einer Mittelstellung von ( 62.5 + 125 ) / 2 = 93.75

 
.include "m16def.inc"
 
.equ XTAL = 4000000
 
          rjmp    init

.org OC2addr
          rjmp    Compare_vect

init: 
          ldi      r16, HIGH(RAMEND)     ; Stackpointer initialisieren
          out      SPH, r16
          ldi      r16, LOW(RAMEND)
          out      SPL, r16

          ldi  r16, 0x80
          out  DDRB, r16                ; Servo Ausgangspin -> Output

          ldi  r17, 0                   ; Software-Zähler
            
          ldi  r16, 120
          out  OCR2, r16                ; OCR2 ist der Servowert

          ldi  r16, 1<<OCIE2
          out  TIMSK, r16

          ldi  r16, (1<<WGM21) | (1<<CS22) ; CTC, Prescaler: 64
          out  TCCR2, r16

          sei

main:
          rjmp main

Compare_vect:
          in   r18, SREG
          inc  r17
          cpi  r17, 1
          breq PulsOn
          cpi  r17, 2
          breq PulsOff
          cpi  r17, 10
          brne return
          ldi  r17, 0
return:   out  SREG, r18
          reti

PulsOn:   sbi  PORTB, 0
          rjmp return

PulsOff:  cbi  PORTB, 0
          rjmp return

Ansteuerung mehrerer Servos mittels Timer[Bearbeiten]