/*
  Servo langsam zu verschiedenen Positionen fahren
  Nicht blockierende Variante mit MobaTools
*/

#include <MobaTools.h>

MoToServo myServo;  // create servo object to control a servo
MoToTimer pauseZeit; // Timer fuer die Pausen

byte positions[] = { 100, 60, 180, 20 }; // Tabelle der anzufahrenden Positionen
constexpr byte posZahl = sizeof(positions); // Positionen müssen als byte deklariert werden))
constexpr int warteZeit = 2000;
byte posIx = 0;
bool pauseAktiv = false;
void setup() {
  Serial.begin(115200);
  myServo.attach(8);  // Steckplatz am Board
  myServo.setSpeedTime ( 2000 ); // 2 Sec werden von 0 auf 180 benötiogt
  //Startposition 0
  myServo.write(0);        // erste Position
}

void loop() {
  if ( myServo.moving() == 0 ) {
    // Nur wenn das Servo steht, passiert was
    if ( not pauseAktiv ) {
      // Servo hat Zielpunkt erreicht, aber Pausezeit läuft noch nicht
      pauseZeit.setTime( warteZeit ); //Timer starten
      pauseAktiv = true;
      Serial.println( "Timer starten");
    }
    if ( pauseZeit.expired() ) {
      // Die Pausezeit ist abgelaufen, Servo auf nächsten Punkt bewegen
      myServo.write( positions[posIx]);
      Serial.print("Gehe zu Pos "); Serial.println (positions[posIx] );
      if ( ++posIx >= posZahl ) {
        // Ende der Positionstabelle erreicht, wieder auf 0
        posIx = 0;
      }
      pauseAktiv = false;
    }
  }
}