Servoansteuerung
aus RN-Wissen, der freien Wissensdatenbank
Inhaltsverzeichnis |
Einleitung
Die Ansteuerung von Servos, z.B. mit einem Atmel Mega32, kann auf viele verschiedene Weisen erfolgen. Im folgenden findet ihr eine gut funktionierende Variante. Der Quellcode bezieht sich zwar auf einen Mega32 und BASCOM, aber gerade BASCOM-Code ist leicht verständlich und für andere Programmiersprachen übersetzbar. Es wird absichtlich nicht der BASCOM interne "SERVO" Befehl genutzt, da dieser nur sehr eingeschränkt und unbefriedigend (z.B. für fliegende Anwendungen) funktioniert. Hier eine kurze Zusammenfassung der vorgeschlagenen Servoansteuerung:
- Der 16bit Timer wird mit einem Prescale von 8 genutzt (==> Interruptfrequenz ohne Preload von ~ 30Hz)
- Interrupt-Aktionen: Nacheinander werden die Servos mit ihrem PWM Signal versorgt:
- Port für das erste Servo auf High schalten
- 1 - 2 ms warten
- Port wieder auf Low schalten
- Nächstes Servo bearbeiten.
- Da viele billige Servos am besten funktionieren wenn die Zeit zwischen ihren Signalen ~ 20 ms (= 50Hz) beträgt, sollte nach dem Stellen des letzten Servos noch eine Pause eingelegt werden (ca. 12ms Pause). In einigen Anwendungen kann es aber erforderlich sein auf diese Pause zu verzichten (z.B. bei der Regelung von Brushless Motoren. Hier ist eine besonders hohe Stellfrequenz oftmals wünschenswert). Bei Verzicht auf diese Pause kann eine Refreshrate von 333Hz bis 166 Hz (bei 3 Brushlessreglern) erreicht werden.
Werden andere Quarzfrequenzen benutzt, kann man mit dem Tool rnAVR komfortabel die richtigen Preload und Prescale Werte errechnen: rnAVR im Roboternetz
Quellcode
Der Bascom Quellcode für eine Auflösung von 2000 Schritten:
$regfile "m32def.dat"
$baud = 19200
$crystal = 16000000
$framesize = 64
$swstack = 64
$hwstack = 64
Config Timer1 = Timer , Prescale = 8 'timer für servos
Enable Timer1
Timer1 = 62535
Config Portb = Output
Portb.0 = 0 'hier hängt servo1
Portb.1 = 0 'hier hängt servo2
Portb.2 = 0 'hier hängt servo3
Portb.3 = 0 'hier hängt servo4
On Timer1 Servoirq 'servo
Enable Interrupts
Dim Kanal As Byte
Dim Servo(4) As Word 'min: 61535, mitte 62535, max 63535 = 2000 schritte
Do
Servo(1) = 62535 'Mitte
Servo(2) = 62535 'Mitte
Servo(3) = 62535 'Mitte
Servo(4) = 62535 'Mitte
Loop
Servoirq:
If Kanal = 0 Then
If Portb.0 = 0 Then 'wenn port low
Timer1 = Servo(1) 'dann timer auf entsprechende verzögerung
Portb.0 = 1 'und port anschalten
Else 'das hier passiert erst bei dem darauf folgenden interrupt
Portb.0 = 0 'dann port wieder ausschalten
Incr Kanal 'und den nächsten kanal bearbeiten
End If
End If
If Kanal = 1 Then
If Portb.1 = 0 Then
Timer1 = Servo(2)
Portb.1 = 1
Else
Portb.1 = 0
Incr Kanal
End If
End If
If Kanal = 2 Then
If Portb.2 = 0 Then
Timer1 = Servo(3)
Portb.2 = 1
Else
Portb.2 = 0
Incr Kanal
End If
End If
If Kanal = 3 Then
If Portb.3 = 0 Then
Timer1 = Servo(4)
Portb.3 = 1
Else
Portb.3 = 0
Incr Kanal
End If
End If
If Kanal = 4 Then
Timer1 = 40000 'eine pause von ca. 12ms bis zum nächsten interrupt. Bei guten Servos oder Brushlessreglern kann man hier bis auf 65530 gehen ==> ansteuerfrequenz von ~ 200Hz
Kanal = 0
End If
Return
End
Empfängersignal durch µC durchschleifen
Oftmals ist auch gewünscht das Signal eines RC-Empfängers einzulesen, die Daten zu bearbeiten/ mischen und dann wieder an Servos auszugeben. Den Code dafür findet ihr im folgenden (das gleiche wie oben aber erweitert mit der Empfängerauswertung). Hier werden die Servos weiterhin mit einer Auflösung von 2000 Schritten angesteuert, die Auflösung der Empfängerauswertung beträgt aber "nur" 72 Schritte. Meiner bescheidenen Meinung nach ist das für eine präzise Steuerung ausreichend. Oftmals werden die Empfängersignale nicht einfach durchgeschleift sondern untereinander bzw. mit Gyrosignalen gemischt. In diesen Fällen fällt die geringere Auflösung der Empfängerauswertung nicht mehr ins Gewicht.
'RC-Signal durch Empfänger durchschleifen:
'Der Empfänger wird mit einer Auflösung von 74 Schritten abgefragt,
'die Servos werden mit einer Auflösung von 2000 Schritten angesteuert.
'Dabei ist die Wiederholfrequenz des Servosignals frei wählbar (35 bis zu 200 Hz).
$regfile "m32def.dat"
$baud = 19200
$crystal = 16000000
$framesize = 64
$swstack = 64
$hwstack = 64
Config Timer0 = Timer , Prescale = 256 , Capture Edge = Falling , Noise Cancel = 1 'empfänger
Config Timer1 = Timer , Prescale = 8 'servo
Enable Timer0
Enable Timer1
Timer1 = 62535
Config Portb = Output
Portb.0 = 0 'hier hängt motor1
Portb.1 = 0 'hier hängt motor2
Portb.2 = 0 'hier hängt motor3
Portb.3 = 0 'hier hängt gierservo4
On Timer0 Pausenerkennung 'empfänger
On Timer1 Servoirq 'servo
Config Int1 = Falling 'empfängersignal angeschlossen an INT1
Enable Int1 'empfänger
On Int1 Summensignalmessung 'empfänger
Enable Interrupts
Dim Empf(5) As Word 'original daten ausm empfänger gehen von min = 63, mitte = 100, max = 137
Dim Sempf(5) As Integer '"nachbearbeitete" empfängerdaten gehen von -999 bis +999 (mit 0 als mitte)
Dim Channel As Byte
Dim Kanal As Byte
Dim Servo(4) As Word 'min: 61535, mitte 62535, max 63535 = 2000 schritte
Dim I As Byte
Do
For I = 1 To 5
Sempf(i) = Empf(i)
Sempf(i) = Sempf(i) - 100 'empfängerwerte mitte auf null verschieben
Sempf(i) = Sempf(i) * 27 'und hochskalieren
Next
Servo(1) = 62535 + Sempf(1)
Servo(2) = 62535 + Sempf(2)
Servo(3) = 62535 + Sempf(3)
Servo(4) = 62535 + Sempf(4)
Loop
Summensignalmessung: 'bei fallender flanke
Select Case Channel
Case 1 :
Empf(1) = Timer0
Case 2 :
Empf(2) = Timer0
Case 3 :
Empf(3) = Timer0
Case 4:
Empf(4) = Timer0
Case 5:
Empf(5) = Timer0
End Select
Timer0 = 6 'preload für 4ms
Incr Channel
Return
Pausenerkennung:
Channel = 0
Return
Servoirq:
If Kanal = 0 Then
If Portb.0 = 0 Then 'wenn port low
Timer1 = Servo(1) 'dann timer auf entsprechende verzögerung
Portb.0 = 1 'und port anschalten
Else 'das hier passiert erst bei dem darauf folgenden interrupt
Portb.0 = 0 'dann port wieder ausschalten
Incr Kanal 'und den nächsten kanal bearbeiten
End If
End If
If Kanal = 1 Then
If Portb.1 = 0 Then
Timer1 = Servo(2)
Portb.1 = 1
Else
Portb.1 = 0
Incr Kanal
End If
End If
If Kanal = 2 Then
If Portb.2 = 0 Then
Timer1 = Servo(3)
Portb.2 = 1
Else
Portb.2 = 0
Incr Kanal
End If
End If
If Kanal = 3 Then
If Portb.3 = 0 Then
Timer1 = Servo(4)
Portb.3 = 1
Else
Portb.3 = 0
Incr Kanal
End If
End If
If Kanal = 4 Then
Timer1 = 40000 'eine pause von ca. 12ms bis zum nächsten interrupt. Bei guten Servos oder Brushlessreglern kann man hier bis auf 65530 gehen ==> ansteuerfrequenz von ~ 200Hz
Kanal = 0
End If
Return
End
