Mit BASCOM bis zu 6 Servos gleichzeitig ansteuern

Atmega 328 mit BASCOM und FORTH programmieren
Antworten
Heinrichs
Beiträge: 173
Registriert: Do 21. Okt 2010, 18:31

Mit BASCOM bis zu 6 Servos gleichzeitig ansteuern

Beitrag von Heinrichs » Do 26. Sep 2019, 20:44

An anderer Stelle habe ich bereits dargestellt, wie man ein einzelnes Servo mit einem Mikrocontroller steuert. Hier wurde die Pulsweitensteuerung mit Hilfe des PWM-Modes 15 des 16-Bit-Timers Timer1 realisiert. Leider besitzt der Atmega238 nur einen einzigen solchen 16-Bit-Timer. Deswegen kann man nicht eine solche automatische PWM-Puls-Generierung benutzen, wenn man gleichzeitig bis zu 6 Servos steuern möchte.

Wie kann man jetzt in diesem Fall vorgehen? Zentrales Element des Zeitmanagements ist wieder der Timer1. Die Pulse für die einzelnen Servos müssen wir aber selbst mit unserem Programm erzeugen. Die Idee ist nun folgende:

Wir beginnen mit einem Puls (von z. B. 1,0 ms) für das Servo 1. Dazu setzen wir den entsprechenden Ausgang (z. B. PortD.2) auf 1. Nun starten wir den Timer1; dabei benutzen wir einen solchen Wert für den Prescale und den Preset, dass es nach dem Ablauf der Pulszeit von 1,0 ms zu einem Timer-Overflow kommt. Dieser Overflow löst einen Timer-Overflow-Interrupt aus.

Die zugehörige Interrupt-Prozedur macht nun folgendes: Zunächst setzt sie den Ausgang für das aktuelle Servo (also PortD.2) auf 0. Anschließend setzt sie den Ausgang für das nächste Servo (z. B. PortD.3 für Servo 2) auf 1 und startet nun den Timer1 erneut, diesmal mit einem Preset, der ihn nach der gewünschten Pulszeit (z. B. 1,5 ms) wieder zum Overflow bringt. Nun wird der Ausgang PortD.3 auf 0 gesetzt; damit ist nun der Puls für das Servo 2 erzeugt. Weiter geht es auf die gleiche Weise mit den Pulsen für die Servos 3 bis 6. Bevor wir wieder mit dem Puls für das Servo 1 beginnen, muss die restliche Zeit bis zum Ende der Periodendauer (20 ms) noch mit einem Null-Signal ausgeführt werden. Dazu setzen wir ein "inaktives" Servo 7 ein, dessen "Pulsdauer" der Restzeit entspricht. Genauere Informationen kann man dem kommentierten Source-Code entnehmen:

Code: Alles auswählen

' Datei für Arduino-UNO/Nano-Board
' steuert bis zu 6 Servos an PortD.2-PortD.7
'----------------------------------------------------------------------------

$regfile = "m328pdef.dat"                                   'Dadurch wird auch das Pin Layout bestimmt
$crystal = 16000000
$framesize = 32
$swstack = 32
$hwstack = 64
$baud = 9600

'**********************************************************
'******************* Deklarationen ************************

Dim Portnr(7) As Byte
Dim Aktiv(7) As Byte
Dim Pulsweite(7) As Word
Dim I As Byte
Dim Eigenschaft As Byte
Dim Wert As Byte
Dim K As Byte
Dim Temp As Byte
Dim Tempword As Word
Dim Pnr As Byte                                             'aktuelle Portnummer
Dim Servonr As Byte
Dim Gesamt As Word
Dim Aktiv_bit As Bit
Dim Zustand_bit As Bit

Declare Sub Servosetzen(byval S_nr As Byte , Byval S_portnr As Byte , Byval S_aktiv As Byte , Byval S_pulsweite As Word )
Declare Sub Servozeigen(byval S_nr As Byte)
Declare Sub Restzeitsetzen
Declare Sub Pulsweitesetzen(byval S_nr As Byte , Byval S_pulsweite As Byte)
Declare Sub Servosteuerung

'****************** Initialisierung ***********************

Ddrd = &B11111100                                           'Port D.2-D.7 als Ausgangsport für die Servos

'Timer1:
On Timer1 Timeroverflow

'**********************************************************
'******************** Hauptprogramm ***********************

Call Servosetzen(1 , 6 , 0 , 0)                             'Parameter: Servo-Nr., Portnr. (bei Port D), Aktiv (0/1), Pulsweite (60 -> 1 ms)
Call Servosetzen(2 , 2 , 1 , 60)                            'etwa 1 ms Pulsweite
Call Servosetzen(3 , 3 , 1 , 90)
Call Servosetzen(4 , 4 , 1 , 120)                           'etwa 2 ms Pulsweite
Call Servosetzen(5 , 5 , 0 , 0)
Call Servosetzen(6 , 7 , 0 , 0)
Call Servosetzen(7 , 0 , 0 , 0)                             'Servo 7 immer inaktiv; Pulsweite (=Restzeit) wird hier vom Programm berechnet
Call Restzeitsetzen

' Einzelne Servo-Daten über Serielle Schnittstelle anzeigen lassen (Text)
' Call Servozeigen(1)
' Call Servozeigen(2)
' Call Servozeigen(7)

Servonr = 1
Pnr = Portnr(servonr)
Portd.pnr = 1
Tcnt1 = 65535 - Pulsweite(servonr)                          'Zählerwert (Preset)
Tccr1b = &B00000100                                         'clk/256 = 62,5 kHz; 256 counts = 4,096 ms
Enable Timer1                                               'Timer1-Overflow ein
Enable Interrupts

Do
'  Servosteuerung
Loop

'**********************************************************
'******************* Unterprogramme ***********************

Sub Servosetzen(byval S_nr As Byte , Byval S_portnr As Byte , Byval S_aktiv Byte , Byval S_pulsweite As Byte )
  Portnr(s_nr) = S_portnr
  Aktiv(s_nr) = S_aktiv
  Pulsweite(s_nr) = S_pulsweite
End Sub

Sub Servozeigen(byval S_nr As Byte)
  Temp = Portnr(s_nr)
  Print Temp ; "-";
  Temp = Aktiv(s_nr)
  Print Temp ; "-";
  Tempword = Pulsweite(s_nr)
  Print Tempword
End Sub

Sub Restzeitsetzen
  Gesamt = 0
  For I = 1 To 6
    Gesamt = Gesamt + Pulsweite(i)
  Next I
  Pulsweite(7) = 1200 - Gesamt                              'Restphase (aktiv = 0)
End Sub

Sub Pulsweitesetzen(byval S_nr As Byte , Byval S_pulsweite As Byte)
    Pulsweite(s_nr) = S_pulsweite
    Call Restzeitsetzen
End Sub

Sub Servosteuerung                                          'Beispiel
  Call Pulsweitesetzen(2 , 60)
  Wait 1
  Call Pulsweitesetzen(4 , 120)
  Wait 1
  Call Pulsweitesetzen(2 , 120)
  Wait 1
  Call Pulsweitesetzen(4 , 60)
  Wait 1
End Sub

'**********************************************************
'******************Interruptroutinen***********************

Timeroverflow:
'Dauer der Interrupt-Routine 9-12 us, davon ca. 6 us für die beiden Portd.x-Anweisungen
'aktuelles Servo:
  Pnr = Portnr(servonr)
  Portd.pnr = 0                                             'Servo-Puls beenden
  If Servonr < 7 Then Servonr = Servonr + 1 Else Servonr = 1
'nächstes Servo:
  Pnr = Portnr(servonr)
  Zustand_bit = Aktiv(servonr)                              'Typ-Anpassung
  Portd.pnr = Zustand_bit                                   'neuen Servo-Puls starten, bei inaktiven Servos immer 0-Signal mit Restzeit;
  Tcnt1 = 65535 - Pulsweite(servonr)                        'Zählerwert (Preset)
Return

'**********************************************************

Die folgende Abbildung stellt die Situation aus diesem Programm dar: Aufgezeichnet wurden die Signale an den Ports D.2 bis D.5. Man sieht deutlich, dass die Pulse für die einzelnen Servos nicht gleichzeitig beginnen, sondern aufeinander folgen. Die Periodendauer ist bei allen Servos gleich, sie ist aber etwas kürzer als 20 ms. Die Servos tolerieren aber solche Abweichungen bei der Periodendauer.

servo_array_signale_kanal_2_3_4_5.jpg
Signalverlauf bei den Ausgängen PortD.2 bis PortD.5
servo_array_signale_kanal_2_3_4_5.jpg (18.54 KiB) 11386 mal betrachtet

Im Unterprogramm Servosteuerung ist dargestellt, wie man auf einfache Weise die Position des Servos jederzeit verändern kann. Dabei entspricht ein Pulsweitenwert von 60 [120] einer Pulsweite von 1,0 ms [2,0 ms]. Bei dem Beispiel

Code: Alles auswählen

Sub Servosteuerung                                          'Beispiel
  Call Pulsweitesetzen(3 , 60)
  Wait 1
  Call Pulsweitesetzen(4 , 120)
  Call Pulsweitesetzen(3 , 90)
  Wait 1
  Call Pulsweitesetzen(4 , 60)
  Wait 1
End Sub

bewegen sich die Servos wie in dem folgenden Video:

servo_WMV V8.wmv
Video zum Beispiel
(1022.82 KiB) 1052-mal heruntergeladen

Hinweis: Die in dem Video benutzten Servos vom Typ SG90 sollten nach Datenblatt bei einer Pulsweite von 1,0 ms auf 3 Uhr und bei einer Pulsweite von 2,0 ms auf 9 Uhr stehen. In Wirklichkeit sind für diese Positionen deutlich kleinere/größere Pulsweiten erforderlich. Zudem kann man bei den einzelnen Exemplaren in dieser Hinsicht auch eine nicht unerhebliche Streuung beobachten. Dafür sind sie aber in der Anschaffung extrem günstig.

.

Antworten