Abbildung 5: Mit dem Demoprojekt von Codesys lassen sich vorab die verschiedenen Winkelstellungen der Servomotoren ausprobieren.
Nun gilt es, herauszufinden, welche Werte der Maximal- und der Mittelposition des Servos entsprechen – diese Werte variieren von Modell zu Modell. Notieren Sie sich die End- und Mittelwerte. Sie können sich auch Markierungen im Arbeitsbereich des Roboters machen: Damit fällt das Programmieren später leichter. Mithilfe des Demoprojekts lassen sich Werte für alle Positionen ermitteln, die der Roboter später einnehmen soll.
Bei den ersten Tests tritt mitunter ein weiteres Problem mit sehr unangenehmen Nebeneffekten auf: Zieht einer der Servos aus irgendeinem Grund (Blockierung oder Überlast) so viel Strom aus der Spannungsversorgung, dass sie zusammenbricht, arbeitet die Regelelektronik der restlichen Motoren ebenfalls nicht mehr korrekt. Alle Servos bewegen sich plötzlich unkontrolliert und chaotisch hin und her. Um sie zu stoppen, deaktivieren Sie alle PWM-Ausgänge. Anschließend aktivieren Sie die Servos einzeln wieder, um das Problem einzugrenzen.
Ein Labornetzteil mit eingebauter Stromüberwachung hilft bei der Fehlersuche enorm. Ein Indiz dafür, dass etwas nicht stimmt, liefert das tiefe Brummen, das die Servos erzeugen, wenn sie eine Position nicht erreichen können. Auch ein Dauerstrom von mehr als 600 Milliampere bei fester Servoposition weist auf einen Fehler hin. Beim Bewegen fließen mitunter aber auch höhere Ströme. Sobald der Motor steht, sollte er so gut wie keinen Strom mehr aufnehmen. Nach dem Durchspielen der hier beschriebenen Tests geht es an das erste richtige Programm.
Roboterarm in Aktion
Um dem Roboterarm Leben einzuhauchen, teilen Sie ihm über eine Schrittkette mit, wann er eine bestimmte Position anfahren soll. Die in diesem Projekt verwirklichte Schrittkette arbeitet rein zeitgesteuert und wird entsprechend ohne Einflüsse von außen abgearbeitet, immer eine Position nach der anderen. Der vorhergehende Test gab ja schon mal Aufschluss darüber, welche Werte zu bestimmten Positionen des Roboterarms gehören.
Für das Programm kommt die häufig in Industriesteuerungen verwendete, Pascal-ähnliche Programmiersprache ST (Strukturierter Text) zum Einsatz. Das Steuerprogramm nutzt ein zweidimensionales Array, um den kompletten Ablauf zu speichern. Eine Dimension speichert die Nummer des aktuellen Schritts, die zweite die fünf Werte der einzelnen Servos sowie einen Wert für die Zeit, die der Roboter benötigt, um die Position zu erreichen.
Listing 1 zeigt die Variablen, die das Programm verwendet. xInit initialisiert den Controller im PiXtend und darf nicht gelöscht werden. Die Variable lock verhindert, dass die Schrittkette mehrfach startet; timer realisiert die Zeitverzögerung; step speichert den aktuellen Schritt, in dem sich die Schrittkette befindet.
Das Array zur Steuerung ist als Konstante definiert, weil sich die Werte während der Laufzeit nicht ändern. Möchten Sie Änderungen an einer der Konstanten vornehmen, müssen Sie darauf achten, das Programm mit der Option Mit Download einloggen in das PiXtend zu laden: Andernfalls aktualisiert die Software diese Konstanten nicht (Abbildung 6).
Abbildung 6: Ändern Sie Konstanten, müssen Sie sich über den PunktMit Download einloggen anmelden, weil die Software die Modifikationen sonst nicht übernimmt.
Listing 1
VAR xInit :BOOL; // init flag lock :BOOL :=FALSE; step :UINT :=1; timer :TON; END_VAR VAR constant Steps : ARRAY [1..19, 1..6] OF UINT := [ // M4, M3, M5, M2, M1 12000, 8000, 6500, 8000, 8000, 2, 12000, 3000, 6500, 8000, 1000, 2, 12000, 3000, 10000, 8000, 1000, 2, 6000, 3000, 10000, 13000, 1000, 2, 6000, 7000, 10000, 13000, 1000, 2, 6000, 7000, 10000, 13000, 12000, 2, 6000, 3000, 10000, 13000, 12000, 1, 6000, 3000, 9000, 13000, 12000, 1, 6000, 3000, 8000, 13000, 12000, 1, 6000, 3000, 7000, 13000, 12000, 1, 6000, 3000, 6000, 13000, 12000, 1, 6000, 3000, 5000, 13000, 12000, 1, 6000, 3000, 4000, 13000, 12000, 1, 6000, 3000, 3000, 13000, 12000, 1, 6000, 7000, 3000, 13000, 12000, 1, 6000, 7000, 3000, 13000, 1000, 1, 12000, 3000, 3000, 13000, 1000, 1, 12000, 3000, 6500, 13000, 1000, 1, 12000, 8000, 6500, 8000, 8000, 2]; END_VAR
Das eigentliche Programm (Listing 2) beginnt erst hinter dem Kommentar // put your program code here... ab Zeile 7. Den Code davor benötigt das Programm, um die Kommunikation zwischen dem PiXtend und dem Raspberry Pi aufzubauen. Als Erstes fragen Sie den Start-Button-Eingang DI0 ab (Zeile 9). Hier verwendet der Code das Alias für das komplette Eingabe-Byte. Die Zahl nach dem Punkt gibt an, welches Bit Sie abfragen möchten. Wird der Button gedrückt und die Variable lock steht auf false (inaktive Schrittkette), dann schaltet die Variable lock auf TRUE, und die Schrittkette beginnt zu laufen.
Im nächsten If-Block ab Zeile 13 fragt der Code die Variable lock ab. Steht sie auf TRUE, wird der Block abgearbeitet. Zunächst aktiviert der Code dann die PWM-Ausgänge des PiXtend. Danach lädt die Sequenz die Werte für den aktuellen Schritt in die Steuerregister der PWM-Ausgänge. Der Roboter bewegt sich nun in diese Position.
Gleichzeitig mit dem Laden der Register wird timer mit der Wartezeit aus dem Array beladen und gestartet. Nach dem Ablauf von timer erhöht sich die Variable step um 1, und der timer wird deaktiviert. Anschließend beginnt alles von vorn, für den nächsten Schritt in der Kette. Nach dem Abarbeiten aller Schritte wechselt lock wieder auf FALSE, step erhält den Wert 1. Die Schrittkette steht dann für den nächsten Durchlauf bereit.
Der letzte If-Block ab Zeile 38 deaktiviert die PWM-Ausgänge nach dem Beenden der Schrittkette wieder. Das deaktiviert die Servos komplett, womit sie so gut wie keinen Strom mehr benötigen.
Ein Youtube-Video [12] zeigt den Roboterarm in Aktion. Dabei fallen die teilweise etwas ruckeligen Bewegungsabläufe auf. Das liegt daran, dass Servos stets versuchen, die neue Position mit maximaler Geschwindigkeit zu erreichen. Eine Möglichkeit, dieses Ruckeln zu vermindern, besteht darin, die Änderungen der Werte in den PWM-Registern gering zu halten. Das erkennt man ansatzweise in der Szene des Videos, in der der Arm eine Maus transportiert: Das erfolgt mit einigen kleinen Schritten, damit der Arm sie nicht wegschleudert.
Um die Servos generell behutsamer anzusteuern, müsste man das Programm so gestalten, dass es sich mit kleinen Schritten an die Zielpositionen annähert. Das gestaltet sich allerdings etwas aufwendiger und würde den Rahmen dieses Artikels sprengen, wäre aber sicher eine gute Programmierübung für einen verregneten Sonntagnachmittag.
Listing 2
// Initialization of PiXtend V2 -L-
IF NOT xInit THEN
RPI_GPIO_OUT24 := TRUE;
xInit := TRUE;
END_IF
// put your program code here...
IF byDigitalInputs0.0 AND lock = FALSE THEN
lock := TRUE;
END_IF
IF lock THEN
// activate PWM Channels
byPWM0Ctrl0.3 := TRUE;
byPWM0Ctrl0.4 := TRUE;
byPWM1Ctrl0.3 := TRUE;
byPWM1Ctrl0.4 := TRUE;
byPWM2Ctrl0.3 := TRUE;
// Write Values to PWM Output
wPWM0A :=steps[step,1];
wPWM0B :=steps[step,2];
wPWM1A :=steps[step,3];
wPWM1B :=steps[step,4];
wPWM2A :=steps[step,5];
timer(PT:=DINT_TO_TIME(steps[step,6])*1000);
timer.in := TRUE ;
IF timer.Q THEN
step := step +1;
timer.IN:= FALSE;
IF step=21 THEN
lock:=FALSE;
step:=1;
END_IF
END_IF
END_IF
IF NOT lock THEN
byPWM0Ctrl0.3 := FALSE;
byPWM0Ctrl0.4 := FALSE;
byPWM1Ctrl0.3 := FALSE;
byPWM1Ctrl0.4 := FALSE;
byPWM2Ctrl0.3 := FALSE;
END_IF
