![Simple Robot - [Teil 3] - AZ-Delivery](http://www.az-delivery.uk/cdn/shop/articles/simple-robot-teil-3-370453.jpg?v=1679408854&width=1650)
Teil 3 - Stop!
Hallo liebe Bastler,
in den ersten beiden Teilen dieses Projektes haben wir einen einfachen Roboter zusammengebaut und programmiert. Er kann auf Tastendruck loslaufen, jedoch funktioniert das Anhalten nicht sehr zuverlässig. Im dritten Teil möchten wir dieses Problem lösen. Los geht's.
Benötigte Hardware
| Anzahl | Bauteil | Anmerkung |
|---|---|---|
| 1 | Arduino Nano V3 | |
| 4 | SG90 Mikroservo | |
| 1 | PCA9685 16 Kanal 12 Bit PWM Servotreiber | |
| 1 | Breadboard | |
| 1 | Taster | |
| Verbindungskabel | ||
| Spannungsversorgung 5V (Labornetzteil oder ähnliches) | ||
| PC mit Arduino IDE und Internetverbindung |
Vorbereitung
Ich gehe davon aus, dass die Schaltung aus Teil 2 noch aufgebaut ist. Wir untersuchen nun, warum der Taster den Roboter nicht zuverlässig anhält. Dazu betrachten wir die Hauptschleife im Quellcode aus Teil 2:
void loop() { // Input lesen input = digitalRead(taster_pin); // Entprellen // wenn Taster jetzt AN und vorher AUS und aktuelle Zeit - alte Zeit groesser, // als vorgegebenes Delay if (input == LOW && input_alt == HIGH && millis() - alte_zeit > prell_delay) { Run = !Run; alte_zeit = millis(); } input_alt = input; if (Run) { // Schritt 1: Füße drehen und damit anheben // Servo 0 als Bedingung, aber Servo 0 und 1 drehen // beide Servos führen die gleiche Drehung aus for (; SERVO_POS[0] < SERVO_MIDDLE[0] + SERVO_MAX[0]; SERVO_POS[0]++, SERVO_POS[1]++) { pwm.setPWM(0, 0, SERVO_POS[0]); pwm.setPWM(1, 0, SERVO_POS[1]); delay(tempo); } // Schritt 2: Beine drehen und damit Roboter bewegen // Servo 2 als Bedingung, aber Servo 2 und 3 drehen // beide Servos führen die gleiche Drehung aus for (; SERVO_POS[2] < SERVO_MIDDLE[2] + SERVO_MAX[2]; SERVO_POS[2]++, SERVO_POS[3]--) { pwm.setPWM(2, 0, SERVO_POS[2]); pwm.setPWM(3, 0, SERVO_POS[3]); delay(tempo); } // Schritt 3: Füße jeweils anders herum drehen und damit anheben // Servo 0 als Bedingung, aber Servo 0 und 1 drehen // beide Servos führen die gleiche Drehung aus for (; SERVO_POS[0] > SERVO_MIDDLE[0] - SERVO_MAX[0]; SERVO_POS[0]--, SERVO_POS[1]--) { pwm.setPWM(0, 0, SERVO_POS[0]); pwm.setPWM(1, 0, SERVO_POS[1]); delay(tempo); } // Schritt 4: Beine jeweils in die andere Richtung drehen // und damit Roboter bewegen // Servo 2 als Bedingung, aber Servo 2 und 3 drehen // beide Servos führen die gleiche Drehung aus for (; SERVO_POS[2] > SERVO_MIDDLE[2] - SERVO_MAX[2]; SERVO_POS[2]--, SERVO_POS[3]++) { pwm.setPWM(2, 0, SERVO_POS[2]); pwm.setPWM(3, 0, SERVO_POS[3]); delay(tempo); } } }
Der Eingang für den Taster wird nur zu Beginn der Schleife untersucht. Die Variable Run wird mit LOW definiert, weswegen das Entprellen und die Schleifen zuerst nicht durchlaufen werden, sondern ausschließlich der Tasterpin in sehr kurzen Abständen eingelesen wird. Wird der Taster betätigt, ändert sich das. Es wird entprellt, wenn der Taster gerade gedrückt wurde.
Danach werden die vier Schleifen durchlaufen. Ist die Hauptschleife wieder am Anfang angekommen, wird erst der Tasterpin eingelesen. Wurde er nicht betätigt, wird nicht entprellt, aber die vier Schleifen für die Bewegung werden durchlaufen, denn Run steht noch auf HIGH. Möchte man den Roboter nun anhalten, müsste man genau den Punkt treffen, an dem die Hauptschleife von vorn beginnt.
Wie ändern wir das? Wir müssten zu jedem Zeitpunkt an allen Stellen des Programms den Tasterpin untersuchen und darauf reagieren. Wir könnten die Zeile
input = digitalRead(taster_pin);
in alle Bewegungsschleifen kopieren. Wir müssten dann den Taster entprellen und die jeweilige Schleife verlassen, wenn er gedrückt wurde. Außerdem müssten wir dafür sorgen, dass die anderen Schleifen danach nicht mehr durchlaufen werden.
Hinweis: Das wiederholte Prüfen des Tasterpins nennt sich "Polling" (engl.).
Interrupts
Eine der verschiedenen Lösungen sind Interrupts. Die Pins D2 und D3 des Arduino Nano können als externe Interrupts genutzt werden. Aber ist das sinnvoll? Interrupt bedeutet Unterbrechung. Das Hauptprogramm wird verlassen und eine sogenannte Interrupt Service Routine (ISR) ausgeführt.
Das ist in diesem Fall eine Funktion, die dann aufgerufen wird, wenn der Interrupt erkannt wurde. In dieser Funktion dürfen normalerweise nur sehr wenige Befehle ausgeführt werden. Kommandos wie delay(), millis() oder micros() sollten vermieden werden, da sie selbst interruptgesteuert sind, deren Interrupts jedoch für den Moment deaktiviert werden.
Es ist noch möglich, millis() auszuführen und die letzten Werte auszulesen. Mehr Informationen dazu findet man in der Arduino-Referenz. Man sollte die ISR immer sehr kurz halten. Ideal sieht sie folgendermaßen aus:
volatile bool flag = LOW; void InterruptServiceRoutine() { flag = !flag; }
Die Variable flag muss volatile deklariert sein. Das bedeutet, dass sie statt in ein Register in den Arbeitsspeicher geschrieben wird. Es wird dafür gesorgt, dass der Inhalt nicht weggeräumt wird, wenn die Daten scheinbar nicht mehr gebraucht werden. Außerdem ist es wichtig, dass die Variable global deklariert ist. Sonst funktioniert der Austausch außerhalb der Funktion nicht.
In der Arduino IDE werden Interrupts mit der Zeile
attachInterrupt(digitalPinToInterrupt(taster_pin), InterruptServiceRoutine, CHANGE);
im setup() initialisiert. Der erste Parameter ist der genutzte Interrupt. In diesem Beispiel wird der Tasterpin mit dem Interrupt verknüpft. Als Nächstes wird der Name der ISR angegeben, die aufgerufen wird, wenn der Interrupt erkannt wurde. Als dritter Parameter wird angegeben, auf was reagiert werden soll. Hier sind der Zustand LOW, die Flanken RISING und FALLING sowie die Änderung CHANGE möglich. Auf einigen Boards auch der Zustand HIGH.
Hat man den Interrupt initialisiert und eine dazugehörige ISR eingefügt, kann man in der Hauptschleife auf den Wert der Austauschvariablen reagieren. In diesem Beispiel ist es die Variable flag. Möchte man zum Beispiel eine LED ein- oder ausschalten, schreibt man einfach den Wert auf einen digitalen Ausgang mit
digitalWrite(ledPin, flag);
Das Beispiel in der Arduino-Referenz funktioniert genau auf diese Weise. Man liest nun keinen Tasterpin mit digitalRead() aus. Während der Ausführung der Hauptschleife kann willkürlich ein Interrupt auftreten, wenn der Taster betätigt wird. Dann ändert sich der Wert der Variable und damit auch der Zustand der LED. Wie sieht das nun aus, wenn wir statt einer LED eine Schleife laufen lassen? Ich habe dazu ein kleines Beispiel geschrieben.
// States volatile bool Run = LOW; // Input int taster_pin = 2; void InterruptServiceRoutine() { Run = !Run; } void setup() { Serial.begin(115200); pinMode(taster_pin, INPUT_PULLUP); attachInterrupt(digitalPinToInterrupt(taster_pin), InterruptServiceRoutine, FALLING); // interrupt pin 2 mit ISR verbinden Serial.println("Bitte Taster betaetigen..."); } void loop() { if (Run) { for(int i = 0; i < 100000; i++) { Serial.println(i); while(!Run) { delay(10); } } } }
Der Taster wird mit dem internen Pull-up-Widerstand als LOW-active initialisiert. Also wird auch die fallende Flanke als Trigger für den Interrupt verwendet. In der Hauptschleife wird ein Zähler gestartet, wenn die Variable Run auf HIGH gestellt wird. Zu Beginn wird sie mit dem Wert LOW definiert. Also passiert bei Programmstart nichts (Sichtbares). Wird der Taster betätigt, wird die Hauptschleife unterbrochen und die ISR-Funktion ausgeführt. Darin ändert sich der Wert der Variable Run von LOW auf HIGH.
Danach wird sie wieder verlassen und die Hauptschleife läuft weiter. Nun wird die for-Schleife durchlaufen, da die if-Bedingung erfüllt ist. Öffnen wir den seriellen Monitor, können wir sehen, wie der Zähler inkrementiert (hochgezählt) wird. Betätigt man den Taster erneut, wird der Zähler pausiert. Lässt man ihn los, wird weiter gezählt. Wir haben eine Pausetaste programmiert.
Damit ist in der Basis der Interrupt erklärt. Stellen wir uns nun vor, unser Programm ist etwas länger. Wir haben mehr Befehle und Funktionen. Wir drücken den Taster, die ISR wird ausgeführt, unsere Variable ändert ihren Wert.
An welcher Stelle fragen wir die Variable dann in der Hauptschleife ab? Sagen wir, wir fragen zu Beginn der loop()-Funktion. Also stünde dort im Gegensatz zu unserem ursprünglichen Programm nur ein anderer Befehl. Es wäre aber wieder der gleiche Programmablauf wie vorher. Wir müssten also noch mehr Änderungen in der ISR durchführen, damit wir nicht mehr regelmäßig pollen müssten. An dieser Stelle fällt dann die Entscheidung, ob ein Interrupt sinnvoll ist.
In unserem und auch in vielen anderen Fällen der Arduinoprogrammierung brauchen wir keinen Interrupt. Das Anhalten des Roboters ist zwar eine zeitkritische Aktion, denn in einem Gefahrenfall muss er an Ort und Stelle stehen bleiben. Allerdings können wir das ohne Interrupts umsetzen.
Hinweis: Ich möchte Sie dazu animieren, trotzdem einmal das Roboter-Programm umzuschreiben und mit Interrupts zu testen.
Stehenbleiben lernen
In Teil 2 dieser Reihe haben wir dem Roboter das Laufen beigebracht. Jetzt lehren wir ihn das sofortige Stehenbleiben. Wir müssen uns nun entscheiden, ob die Bewegung einfach nur pausieren oder auf den Ausgangszustand gesetzt werden soll. Da in unserem Fall ein Reset des Arduino Nanos dazu führt, dass die Bewegung zurückgesetzt wird, möchte ich eine Bewegungsunterbrechung implementieren. Das heißt, dass man den Taster betätigt und der Roboter friert ein. Eine erneute Betätigung des Tasters lässt ihn weiterlaufen.
Zu Beginn dieses Beitrags hatte ich bereits erwähnt, dass wir einfach in jeder Bewegungsschleife den Tasterpin auslesen können. Er sollte dann noch entprellt werden und das gedrückt Halten des Tasters sollte ebenfalls herausgefiltert werden.
Unser Hauptproblem ist, dass der Taster nur zu Beginn der Hauptschleife untersucht wird. Befindet sich das Programm in den Bewegungsschleifen, können wir im Grunde nichts weiter tun. Wir könnten sie mit break verlassen, wenn der Taster betätigt wurde. Also müssen wir zu jeder Zeit abfragen.
Eleganter wäre es, wenn wir uns gar nicht in solch einer Schleife befänden. Wir brauchen aber einen Zyklus, damit wir die Servos schrittweise weiter drehen können. Die Hauptschleife ist schon eine Schleife. Warum also Schleifen in Schleifen programmieren? Wir können eine Statemachine (engl., Statusmaschine oder Zustandsmaschine) programmieren.
Man kann sich das wie eine Modelleisenbahn vorstellen, die immer im Kreis fährt. Wir haben verschiedene Weichen, damit die Bahn andere Kreise fahren kann. Sie fährt aber immer wieder am selben Hauptbahnhof vorbei. Unser Programm fährt durch eine Teilstrecke so lange, bis die jeweilige Bewegung vollendet ist. Danach ändert sich der Status, eine Weiche wird umgestellt und die nächste Teilstrecke wird durchfahren.
Da wir jetzt nur noch die Hauptschleife durchlaufen, können wir nach jedem Servoschritt den Taster abfragen. Das ganze nennt sich dann "nichtblockierendes Programm" und ist für eingebettete (Echtzeit-) Systeme sehr wichtig.
Wir fügen für eine weitere Variable folgende Zeile ein:
unsigned int state = 0;
Der Zähler für die Statemachine wird niemals negativ, daher deklarieren wir ihn vorzeichenlos. Wir brauchen sehr wahrscheinlich auch nicht so viele Zustände, wie ein Integer es zulässt. Wir belassen es aber erst einmal dabei.
Die folgende Zeile können wir entfernen:
int input_analog = 0;
Sie fing ursprünglich die Werte des Potentiometers auf. Kommen wir nun zum wichtigsten Teil des Programms. Alles was innerhalb der Bedingung
if (Run) {
steht, werden wir jetzt verändern. Bisher haben wir dort vier Schleifen. Sie zählen in den jeweiligen Positionsarrays die Werte hoch oder runter. Für jede Schleife schreiben wir nun einen Zustand in der Statemachine. Dafür nutzen wir eine switch-case-Anweisung. Als Bedingung fügen wir unsere neue Variable state ein. Bauen wir zuerst das Grundgerüst:
switch (state) { case 0: break; case 1: break; case 2: break; case 3: break; default: case = 0; break; }
Hinweis: Die Anweisung break darf nicht vergessen werden, da sonst der Code im nächsten Zustand ebenfalls ausgeführt wird. Außerdem kann es nicht schaden, mit default die Werte der Variable state sicherer einzugrenzen.
Die Bewegung unseres Roboters besteht aus vier Schritten. Diese werden nun durch die Zustände 0 bis 3 repräsentiert. Der Startwert der Variable state ist zu Beginn 0. Die Hauptschleife läuft also endlos durch diesen Zustand. Die anderen Zustände werden ignoriert. Um die Statemachine "weiterzuschieben", müssen wir state einen anderen Wert geben. Das sollten wir nur mit einer Bedingung tun.
Innerhalb des ersten Zustandes können wir mit jedem Durchlauf der Hauptschleife die Servopositionen sukzessive hoch- oder runterzählen. Allerdings nur innerhalb unserer Bewegungsgrenzen. Sind diese erreicht, schieben wir die Statusmaschine weiter. Also sind die Bewegungsgrenzen die Bedingung für die Änderung der state-Variable.
Eine weitere wichtige Sache ist, dass sich die Art der Bedingung ändert. Vorher hießen diese:
Solange die Werte unter meiner Bewegungsgrenze liegt, zähle hoch und sende den aktuellen Wert an das Servo.
Nun lauten sie:
Wenn die Werte gleich oder höher meiner Bewegungsgrenze liegen, schiebe die Statemachine weiter.
Wir müssen also die "größer als"- und "kleiner als"-Zeichen umdrehen.
Hier der geänderte Quellcode:
if (Run) { switch(state) { case 0: { pwm.setPWM(0, 0, SERVO_POS[0]); pwm.setPWM(1, 0, SERVO_POS[1]); delay(tempo); SERVO_POS[0]++; SERVO_POS[1]++; if (SERVO_POS[0] >= SERVO_MIDDLE[0] + SERVO_MAX[0]) { state++; } }; break; case 1: { pwm.setPWM(2, 0, SERVO_POS[2]); pwm.setPWM(3, 0, SERVO_POS[3]); delay(tempo); SERVO_POS[2]++; SERVO_POS[3]--; if (SERVO_POS[2] >= SERVO_MIDDLE[2] + SERVO_MAX[2]) { state++; } }; break; case 2: { pwm.setPWM(0, 0, SERVO_POS[0]); pwm.setPWM(1, 0, SERVO_POS[1]); delay(tempo); SERVO_POS[0]--; SERVO_POS[1]--; if (SERVO_POS[0] <= SERVO_MIDDLE[0] - SERVO_MAX[0]) { state++; } }; break; case 3: { pwm.setPWM(2, 0, SERVO_POS[2]); pwm.setPWM(3, 0, SERVO_POS[3]); delay(tempo); SERVO_POS[2]--; SERVO_POS[3]++; if (SERVO_POS[2] <= SERVO_MIDDLE[2] - SERVO_MAX[2]) { state = 0; } }; break; default: state = 0; break; } }
Ähnlich wie im Quellcode aus Teil 2 senden wir die Positionswerte an das jeweilige Servo. Danach zählen wir die Positionszähler weiter. Anschließend fügen wir wieder eine kurze Pause ein, die die Bewegungsgeschwindigkeit festlegt. Ist die jeweilige Bewegungsgrenze erreicht, schieben wir die Statemachine weiter.
Damit haben wir wieder vier Stufen für die Bewegung, die aber nicht für sich gekapselt sind. Dadurch wird nach jeder Positionsänderung eines Servos auch schon direkt der Tasterpin abgefragt und ausgewertet. Wird dieser betätigt, hält der Roboter sofort an.
Die Software
/* Simple Robot * von Andreas Wolter * fuer AZ-Delivery.de * * Version: 2.0 * * Funktion: * Mit Servos einen einfachen Roboter zum Laufen bringen. * Mit Taster die Bewegung starten und stoppen. * * Changelog: * - nichtblockierende Statemachine fuer sofortiges * Anhalten * * Verwendete Hardware: * - Arduino Nano V3 * - SG90 Mikroservos (4x) * - PCA9685 16 Kanal 12 Bit PWM Servotreiber * - Taster * - externe Spannungsversorgung 5V * * Verwendete Bibliotheken: * - wire * - Adafruit PWM Servo Driver Library * * Beispielquelle aus der Adafruit PWM Servo Driver Library: servo * *************************************************** This is an example for our Adafruit 16-channel PWM & Servo driver Servo test - this will drive 8 servos, one after the other on the first 8 pins of the PCA9685 Pick one up today in the adafruit shop! ------> http://www.adafruit.com/products/815 These drivers use I2C to communicate, 2 pins are required to interface. Adafruit invests time and resources providing this open source code, please support Adafruit and open-source hardware by purchasing products from Adafruit! Written by Limor Fried/Ladyada for Adafruit Industries. BSD license, all text above must be included in any redistribution **************************************************** * * Pinout: * * Arduino Nano | Servo Treiber | Externe Spannungsquelle * ------------------------------------------------------------- * GND | GND | * 5V | VCC | * A4 | SDA | * A5 | SCL | * | Connector V+ | +5V * | Connector GND | GND * * Arduino Nano | Input * ------------------------------------------------------------- * D8 | Taster Pin 1 * GND | Taster Pin 2 */ #include <Wire.h> #include <Adafruit_PWMServoDriver.h> #define SERVOMIN 68 // 0 bis 4096 - try and error #define SERVOMAX 510 // 0 bis 4096 - try and error #define SERVO_FREQ 50 // Analog servos run at ~50 Hz updates #define TOLERANZ 15 // Prozent vom Endanschlag Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(); // States bool Run = LOW; unsigned int state = 0; // Input int taster_pin = 8; bool input = HIGH; bool input_alt = HIGH; // Servos // Berechne Min und Max mit Servotoleranz am Aussenanschlag const int MAXSERVOS = 4; int range = SERVOMAX - SERVOMIN; double temp = (range / 100) * (double)TOLERANZ; int NEWMIN = SERVOMIN + (int)temp; int NEWMAX = SERVOMAX - (int)temp; int i = 0; unsigned long tempo = 15; int SERVO_MIDDLE[MAXSERVOS] = {280, 316, 356, 284}; int SERVO_MAX[MAXSERVOS] = {35, 35, 35, 35}; // Bewegungsradius eingrenzen int SERVO_POS[MAXSERVOS] = {0}; // aktuelle Servopositionen // Timer unsigned long prell_delay = 100; unsigned long alte_zeit = 0; void setup() { Serial.begin(115200); Wire.begin(); pinMode(taster_pin, INPUT_PULLUP); pwm.begin(); pwm.setOscillatorFrequency(27000000); // The int.osc. is closer to 27MHz pwm.setPWMFreq(SERVO_FREQ); // Analog servos run at ~50 Hz updates delay(500); // auf Grundposition einstellen for (i = 0; i < MAXSERVOS; i++) { SERVO_POS[i] = SERVO_MIDDLE[i]; pwm.setPWM(i, 0, SERVO_POS[i]); } } void loop() { // Input lesen und anpassen input = digitalRead(taster_pin); // Entprellen // wenn Taster jetzt AN und vorher AUS und aktuelle Zeit - alte Zeit groesser, als vorgegebenes Delay if (input == LOW && input_alt == HIGH && millis() - alte_zeit > prell_delay) { Run = !Run; alte_zeit = millis(); } input_alt = input; if (Run) { switch(state) { case 0: { pwm.setPWM(0, 0, SERVO_POS[0]); pwm.setPWM(1, 0, SERVO_POS[1]); delay(tempo); SERVO_POS[0]++; SERVO_POS[1]++; if (SERVO_POS[0] >= SERVO_MIDDLE[0] + SERVO_MAX[0]) { state++; } }; break; case 1: { pwm.setPWM(2, 0, SERVO_POS[2]); pwm.setPWM(3, 0, SERVO_POS[3]); delay(tempo); SERVO_POS[2]++; SERVO_POS[3]--; if (SERVO_POS[2] >= SERVO_MIDDLE[2] + SERVO_MAX[2]) { state++; } }; break; case 2: { pwm.setPWM(0, 0, SERVO_POS[0]); pwm.setPWM(1, 0, SERVO_POS[1]); delay(tempo); SERVO_POS[0]--; SERVO_POS[1]--; if (SERVO_POS[0] <= SERVO_MIDDLE[0] - SERVO_MAX[0]) { state++; } }; break; case 3: { pwm.setPWM(2, 0, SERVO_POS[2]); pwm.setPWM(3, 0, SERVO_POS[3]); delay(tempo); SERVO_POS[2]--; SERVO_POS[3]++; if (SERVO_POS[2] <= SERVO_MIDDLE[2] - SERVO_MAX[2]) { state = 0; } }; break; default: state = 0; break; } } }
Vorschau
Einigen ist sicher aufgefallen, dass wir noch die delay()-Funktion verwenden. Auch das blockiert natürlich unser Programm. Das ändern wir im nächsten Teil. Außerdem wollen wir dann auch endlich das Potentiometer wieder mit einbeziehen und die Bewegungsgeschwindigkeit verändern. Auch dafür ist es wichtig, dass der Programmablauf nicht blockiert.
