Teil 4 - Tempo! 

Hallo liebe Bastler,

nachdem wir unseren Roboter etwas besser unter Kontrolle gebracht haben, möchten wir die Geschwindigkeit während der Bewegung verändern. Außerdem gibt es noch einige Stellen im Code, die den Programmablauf blockieren, und optimieren wollen wir ihn auch noch etwas. Los geht's.

Benötigte Hardware 

Vorbereitung

Ich gehe davon aus, dass die Schaltung aus Teil 3 noch aufgebaut ist. Im ersten Teil hatten wir das Potentiometer genutzt, um die Mittelposition der Servoachsen zu ermitteln. Nun nutzen wir es, um die Bewegungsgeschwindigkeit zu verändern. Es sollte noch am Pin A0 des Arduino angeschlossen sein. Wenn nicht, holen Sie das an dieser Stelle bitte nach. Die äußeren Kontakte an +5V und GND. Der mittlere Kontakt ist meistens der "Schleifer", der den Widerstand verändert. Dieser wird wie gesagt an Pin A0 angeschlossen.

Bewegungsgeschwindigkeit ändern

Untersuchen wir noch einmal unseren Quellcode und finden die Stellen, an denen wir die Geschwindigkeit verändern. Wir haben bereits eine passende Variable mit dem Namen tempo deklariert und mit dem Wert 15 definiert. In der Hauptschleife in unserer Statemachine, die durch die switch-case-Anweisung dargestellt wird, finden wir mehrmals die Zeile:

delay(tempo);

Es wird also in jedem Durchlauf eine Pause von 15 ms eingelegt, nachdem der Wert für die Position an das jeweilige Servo gesendet wurde. Wir drehen das Servo um einen Schritt weiter, pausieren, drehen wieder einen Schritt und immer so weiter. Verkleinern wir diesen Wert, ist die Pause kürzer, die Servos drehen sich schneller, der Roboter läuft schneller. Vergrößern wir den Wert, läuft er folglich langsamer.

Mit dem Potentiometer können wir am analogen Eingang des Arduino Mikrocontrollers Werte von 0 bis 1023 aufnehmen. Dieser Bereich ist etwas zu groß. Darum grenzen wir ihn ein. Dafür benötigen wir zwei weitere Variablen für die minimale und die maximale Pause in der Bewegung. Die Variable tempo wird dann zyklisch überschrieben, weswegen wir sie auch mit 0 definieren können:

unsigned long tempo = 0;
unsigned long tempo_min = 5;
unsigned long tempo_max = 20;

Ich habe mit den Werten ein wenig gespielt und für mich 5 und 20 gewählt. Probieren Sie selbst aus, welche Geschwindigkeiten für Sie ok sind.

In unsere Hauptschleife fügen wir wie in Teil 1 die Zeilen für das Einlesen des analogen Tastereingangs ein:

tempo = analogRead(A0);
tempo = map(tempo, 0, 1023, tempo_min, tempo_max);

Wir lesen den Wert am analogen Eingang in die Variable tempo. Dann passen wir den Wertebereich an unsere minimale und maximale Pause an und überschreiben damit erneut diese Variable. Laden wir das Programm auf den Arduino und schalten die externe Stromversorgung ein, sollte der Roboter nach Betätigen des Tasters loslaufen. Drehen wir am Potentiometer, sollte sich sofort die Geschwindigkeit verändern.

Da wir die großen Blockaden aus unserem Programm entfernt haben, wird direkt nach dem Auslesen des analogen Eingangs der entsprechende Wert für die nächste auftretende Zeile, die den delay()-Befehl enthält, verwendet.

Pause ohne delay()

Für Arduinoprogramme, die z.B. in bestimmten Abständen Sensoren auslesen, kann die Funktion delay() verwendet werden. Sie wird auch häufig dafür verwendet. Da wir nun keine parallelen Prozesse auf dem Arduino ausführen können, sind wir auf eine serielle Abarbeitung der Befehle angewiesen. Soll der Mikrocontroller mehrere Aufgaben ausführen, z.B. eine LED blinken lassen, einen Tastereingang prüfen und darauf reagieren, Sensoren auslesen, Aktuatoren auslösen, mit anderen Geräten kommunizieren, dann wird delay() rasch zum Stolperstein.

Das Prinzip, das wir für das Entprellen des Tasters verwendeten, können wir nun auch hierfür nutzen. Wir legen eine Zeitspanne fest, die mindestens überschritten sein muss, um ein Kommando auszuführen. Ist das nicht der Fall, rauscht die CPU einfach daran vorbei.

Wir erzeugen uns eine weitere Variable, die vom Datentypen her zum Rückgabewert der Funktion millis() passt. Sie speichert die vergangene Zeit. Damit können wir die Zeitspanne messen, die von dem vergangenen bis zum aktuellen Zeitpunkt verstrichen ist. Diese vergleichen wir mit dem Wert der Variable tempo. Der ist bei uns auch schon durch das Potentiometer veränderbar. Wir löschen nun alle Zeilen mit dem Inhalt:

delay(tempo);

Als nächsten Schritt ergänzen wir durch ein logisches UND in der Bedingung für die Statemachine die Zeitmessung.

vorher:

if (Run) {

nachher:

if (Run && millis() - servo_zeit_alt > tempo) {

Die Statemachine wird ab jetzt nur dann durchlaufen, wenn zwei Bedingungen erfüllt sind. Die Variable Run muss durch das Betätigen des Tasters wie gehabt auf HIGH stehen. UND die gemessene Zeitspanne muss größer sein als der Wert der Variable tempo. Wir dürfen nicht vergessen, die aktuelle Zeit für den nächsten Durchlauf zu speichern, bevor die if-Anweisung verlassen wird:

servo_zeit_alt = millis();

Damit haben wir das Programm nun von weiteren Zeitblockaden befreit. Würden wir nun einen Sensor implementieren, der Gegenstände detektiert, könnte der Roboter sehr zeitnah darauf reagieren. Eventuell können wir zu einem späteren Zeitpunkt solch eine Erweiterung hinzufügen (im Grunde sind Taster und Potentiometer schon einfache Sensoren).

Codeoptimierung

Mit Blick auf den Code der switch-case-Anweisung sehen wir, dass sich vieles ähnelt oder sogar gleicht. Das ist redundanter Code, den wir optimieren können. Dafür schreiben wir eine Funktion. Alles, was sich zwischen den Cases unterscheidet, müssen wir dieser Funktion dann als Parameter übergeben. Alles, was sich gleicht, schreiben wir in die neue Funktion. An dieser Stelle ist es von Vorteil, dass wir die Servopositionen in den Arrays global deklariert haben. Dadurch können wir innerhalb und außerhalb der Funktion auf diese Daten zugreifen. Wir erzeugen also eine Funktion und nennen sie bewegung().

Die nächste Überlegung ist, ob wir einen Rückgabewert brauchen. Im bisherigen Code sehen wir, dass das Weiterschieben der Statemachine durch die Variable state an die Bedingung geknüpft ist, dass der maximale Bewegungsradius erreicht sein muss. Diese Abfrage wiederholt sich und wird daher in die Funktion verschoben.

Wir brauchen aber das Ergebnis daraus für den Zähler der Statemachine. Also nutzen wir dafür den Rückgabewert der Funktion. Die state-Variable ist eine Ganzzahl, die hochgezählt wird. Addieren wir doch einfach immer die Rückgabe aus der Funktion zur state-Variable hinzu. Ist die Bewegungsgrenze erreicht, addieren wir 1 und schieben damit die Statemachine weiter. Ansonsten addieren wir 0. Somit bleibt state beim gleichen Wert. Wir müssen also in der Funktion dafür sorgen, dass eine 0 oder eine 1 zurückgegeben wird. Unser Rückgabetyp der Funktion ist ein Integer.

Als nächstes brauchen wir Parameter, die der Funktion übergeben werden, damit sie mit deren Werten arbeiten kann. Unsere Cases in der Statemachine unterscheiden sich unter anderem durch die Servonummern. In Case 0 sind es Servo 0 und 1, in Case 1 sind es 2 und 3, in Case 2 sind es wieder 0 und 1 und in Case 3 sind es wieder Servo 2 und 3. Wir übergeben also die beiden Servonummern als die ersten beiden Parameter.

Innerhalb der Funktion können wir überall dort, wo wir bisher die Nummern angegeben haben, die übergebenen Variablen angeben. Die nächsten Punkte sind das Hoch- und Runterzählen der Servopositionen in den Arrays. Wir müssen inkrementieren oder dekrementieren. Das erreichen wir, in dem wir entweder eine 1 oder eine -1 übergeben. Innerhalb der Funktionen addieren wir diese Parameter in den Positionsarrays hinzu. Da plus und minus gleich minus ergibt, erreichen wir dadurch das Dekrementieren.

Wir bewegen in jedem Schritt zwei Servos, zählen dabei auch unterschiedlich hoch oder runter, je nach Bewegungsphase. Wir übergeben folglich die Drehrichtung für beide Servos getrennt. Die Deklaration der Funktion sieht damit folgendermaßen aus:

int bewegung(int servo_1, int servo_2, int dir_1, int dir_2) {}

Rückgabe ist die Addition für die state-Variable, Parameter 1 und 2 sind die Servonummern, Parameter 3 und 4 sind die Bewegungsrichtungen der beiden Servos.

Nun können wir in die Cases der switch-case-Anweisung den Aufruf der Funktion einfügen. Im bisherigen Code sehen wir, dass wir im ersten Zustand die Servos 0 und 1 ansprechen. Das Zählen für beide Positionszähler ist positiv. Also sind die Übergabewerte für beide gleich 1. Die Zeile sieht dann folgendermaßen aus:

state = state + bewegung(0, 1, 1, 1);

Das Ergebnis der Funktion wird zur state-Variablen hinzuaddiert. Sollte die Rückgabe gleich 1 sein, folgt Case 1. Hier sprechen wir die Servos 2 und 3 an. Die Zählrichtung für Servo 2 ist positiv, die von Servo 3 jedoch negativ.
Wir übergeben also an die Funktion die beiden Servonummern 2 und 3 sowie eine 1 für Servo 2 und eine -1 für Servo 3:

state = state + bewegung(2, 3, 1, -1);

So ergeben sich dann auch die Funktionsaufrufe in den letzten beiden Cases. Die Befehle für das Setzen der Servopositionen und das Inkrementieren verschieben wir in unsere bewegen()-Funktion. Die gesamte switch-case-Anweisung sieht dann nur noch folgendermaßen aus:

if (Run && millis() - servo_zeit_alt > tempo) {
    switch(state) {
        case 0: state = state + bewegung(0, 1, 1, 1); break; 
        case 1: state = state + bewegung(2, 3, 1, -1); break;
        case 2: state = state + bewegung(0, 1, -1, -1); break; 
        case 3: state = state + bewegung(2, 3, -1, 1); break;
        default: state = 0; break;
    }
    servo_zeit_alt = millis();
}

Nun müssen wir die Befehle innerhalb der Funktion noch abändern. Wir schreiben nun nicht mehr feste Zahlen für die Servos, sondern übernehmen die übergebenen Parameter:

pwm.setPWM(servo_1, 0, SERVO_POS[servo_1]);
 pwm.setPWM(servo_2, 0, SERVO_POS[servo_2]);

Damit setzen wir die Positionen der Servos anhand ihrer Werte in den Arrays. Fehlt noch das Hoch- oder Runterzählen der Positionswerte:

SERVO_POS[servo_1] = SERVO_POS[servo_1] + dir_1;
 SERVO_POS[servo_2] = SERVO_POS[servo_2] + dir_2;

Als Nächstes müssen wir entscheiden, wann die Statemachine zum nächsten Zustand weitergeschoben wird. Das heißt, wann geben wir eine 1 oder wann eine 0 zurück. Wir betrachten dabei immer nur das erste der beiden zu bewegenden Servos.

In den ersten beiden Cases wird die Position des jeweils ersten Servos hochgezählt. In den anderen beiden Cases runtergezählt. Wir können zwei Fälle unterscheiden. Außerdem brauchen wir dazu noch die jeweiligen Bewegungsgrenzen als nächste Bedingung. In den ersten beiden Cases ist es "größer als", in den anderen beiden Cases "kleiner als". Diese Bedingungen sind voneinander abhängig. Daher verknüpfen wir sie mit einem logischen UND.

Das klingt zuerst einmal etwas kompliziert. Allerdings optimieren wir dadurch den Code. Ich zeige Ihnen hier beide Varianten. Zuerst den kompletten Inhalt der optimierten Funktion:

int bewegung(int servo_1, int servo_2, int dir_1, int dir_2) {
  pwm.setPWM(servo_1, 0, SERVO_POS[servo_1]);
  pwm.setPWM(servo_2, 0, SERVO_POS[servo_2]);
  SERVO_POS[servo_1] = SERVO_POS[servo_1] + dir_1;
  SERVO_POS[servo_2] = SERVO_POS[servo_2] + dir_2;
  if ((dir_1 > 0 && SERVO_POS[servo_1] >= SERVO_MIDDLE[servo_1] + SERVO_MAX[servo_1]) || (dir_1 < 0 && SERVO_POS[servo_1] <= SERVO_MIDDLE[servo_1] - SERVO_MAX[servo_1])) {
    return 1;
  }
  return 0;
}

Die noch nicht optimierte Variante der Funktion könnte so aussehen:

int bewegung(int servo_1, int servo_2, int dir_1, int dir_2) {
  pwm.setPWM(servo_1, 0, SERVO_POS[servo_1]);
  pwm.setPWM(servo_2, 0, SERVO_POS[servo_2]);
  SERVO_POS[servo_1] = SERVO_POS[servo_1] + dir_1;
  SERVO_POS[servo_2] = SERVO_POS[servo_2] + dir_2;
  if (dir_1 > 0 ) {
    if (SERVO_POS[servo_1] >= SERVO_MIDDLE[servo_1] + SERVO_MAX[servo_1]) {
      return 1;
    }
  }
  else if (dir_1 < 0) {
    if (SERVO_POS[servo_1] <= SERVO_MIDDLE[servo_1] - SERVO_MAX[servo_1]) {
      return 1;
    }
  }
  return 0;
}

Man sieht, dass die Zeile für die Rückgabe innerhalb der verschachtelten if-Anweisungen identisch ist. Alles was identisch aussieht, kann man in der Regel zusammenfassen. Das Ergebnis ist für beide Varianten gleich. Die Anzahl der Codezeilen unterscheidet sich jedoch. Die mit UND verknüpften Bedingungen sind nicht für jeden gleich gut lesbar. Es bleibt daher jedem überlassen, wie weit er/sie den Code zusammenfasst und optimiert.

Hinweis: Die Position der Funktion im Quellcode ist in der Arduino-IDE unerheblich. Normalerweise muss man in C bzw. C++ die Funktion dem Compiler bekanntmachen. Daher sollte man sie entweder vor die main()-Funktion schreiben, oder durch Vorwärtsdeklaration zumindest bekanntmachen. Das bedeutet, dass man die allgemeine Deklaration ohne Inhalt und nur mit den verwendeten Datentypen vor die main()-Funktion schreibt. Die setup()-Funktion ist nicht das Gleiche wie eine main()-Funktion.

Die Software 

/*  Simple Robot
 *  von Andreas Wolter
 *  fuer AZ-Delivery.de
 *  
 *  Version: 3.0
 *  
 *  Funktion:
 *  Mit Servos einen einfachen Roboter zum Laufen bringen.
 *  Mit Taster die Bewegung starten und stoppen.
 *  
 *  Changelog:
 *    - nichtblockierende Statemachine fuer sofortiges
 *      Anhalten
 *    - Tempo mit Poti aendern
 *    - Pausen ohne delay()
 *  
 *  Verwendete Hardware:
 *    - Arduino Nano V3
 *    - SG90 Mikroservos (4x)
 *    - PCA9685 16 Kanal 12 Bit PWM Servotreiber
 *    - Taster
 *    - Potentiometer (hier 10KOhm)
 *    - 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
 *      A0        |   Potentiometer Mitte (Schleifer Pin)
 *      5V        |   Potentiometer Außen 1
 *      GND       |   Potentiometer Außen 2
 *      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 = 0;
unsigned long tempo_min = 5;
unsigned long tempo_max = 20;

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;
unsigned long servo_zeit_alt = 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);
  tempo = analogRead(A0);
  tempo = map(tempo, 0, 1023, tempo_min, tempo_max);

  // 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 && millis() - servo_zeit_alt > tempo) {
    switch(state) {
      case 0: {
        pwm.setPWM(0, 0, SERVO_POS[0]);
        pwm.setPWM(1, 0, SERVO_POS[1]);
        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]);
        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]);
        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]);
        SERVO_POS[2]--;
        SERVO_POS[3]++;
        if (SERVO_POS[2] <= SERVO_MIDDLE[2] - SERVO_MAX[2]) {
          state = 0;
        }
      }; break;
      default: state = 0; break;
    }
    servo_zeit_alt = millis();
  }
}

Der gesamte Quellcode nach der Optimierung:

/*  Simple Robot
 *  von Andreas Wolter
 *  fuer AZ-Delivery.de
 *  
 *  Version: 3.1
 *  
 *  Funktion:
 *  Mit Servos einen einfachen Roboter zum Laufen bringen.
 *  Mit Taster die Bewegung starten und stoppen.
 *  
 *  Changelog:
 *    - Codeoptimierung
 *  
 *  Verwendete Hardware:
 *    - Arduino Nano V3
 *    - SG90 Mikroservos (4x)
 *    - PCA9685 16 Kanal 12 Bit PWM Servotreiber
 *    - Taster
 *    - Potentiometer (hier 10KOhm)
 *    - 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
 *      A0        |   Potentiometer Mitte (Schleifer Pin)
 *      5V        |   Potentiometer Außen 1
 *      GND       |   Potentiometer Außen 2
 *      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 = 0;
unsigned long tempo_min = 5;
unsigned long tempo_max = 20;

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;
unsigned long servo_zeit_alt = 0;

int bewegung(int servo_1, int servo_2, int dir_1, int dir_2) {
  pwm.setPWM(servo_1, 0, SERVO_POS[servo_1]);
  pwm.setPWM(servo_2, 0, SERVO_POS[servo_2]);
  SERVO_POS[servo_1] = SERVO_POS[servo_1] + dir_1;
  SERVO_POS[servo_2] = SERVO_POS[servo_2] + dir_2;
  if ((dir_1 > 0 && SERVO_POS[servo_1] >= SERVO_MIDDLE[servo_1] + SERVO_MAX[servo_1])
        || (dir_1 < 0 && SERVO_POS[servo_1] <= SERVO_MIDDLE[servo_1] - SERVO_MAX[servo_1])) {
    return 1;
  }
  return 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);
  tempo = analogRead(A0);
  tempo = map(tempo, 0, 1023, tempo_min, tempo_max);

  // 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 && millis() - servo_zeit_alt > tempo) {
    switch(state) {
      case 0: state = state + bewegung(0, 1, 1, 1); break; 
      case 1: state = state + bewegung(2, 3, 1, -1); break;
      case 2: state = state + bewegung(0, 1, -1, -1); break; 
      case 3: state = state + bewegung(2, 3, -1, 1); break;
      default: state = 0; break;
    }
    servo_zeit_alt = millis();
  }
}

Vorschau

Im nächsten Teil dieses Projektes werden wir versuchen, die Laufrichtung des Roboters in der Bewegung zu ändern. Außerdem wollen wir die Nabelschnur durchtrennen und die Energieversorgung mobil machen. Darüber hinaus plane ich noch, Entfernungssensoren für eine proprietäre Hinderniserkennung zu ergänzen. Bis dahin.

Andreas Wolter

für AZ-Delivery Blog