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

#include <MobaTools.h>

const byte servoPins[] = {7, 6, 5, 4,8,9,10,11};
const byte buttonPins[] = {3, 2, A5, A4,A3,A2,A1,A0};
constexpr byte servoZahl = sizeof(servoPins); // servoPins muss 'byte' sein
constexpr int slowSpeed = 2000;
MoToServo myServo[servoZahl];  // create servo object to control a servo
MoToTimer pauseZeit[servoZahl] ; // Timer fuer die Pausen
MoToButtons myButtons( buttonPins, servoZahl, 20, 500 );

constexpr byte angle0 = 0;
constexpr byte angle1 = 90;
uint32_t servoTime = 3000;    // nach 3 Sekunden wieder zurückfahren

void setup() {
  for ( byte i = 0; i < servoZahl; i++ ) {

    myServo[i].attach(servoPins[i]);  // Steckplatz am Board
    //myServo[i].setSpeedTime ( 1000 ); // 1 Sec werden von 0 auf 180 benötiogt
    //Startposition 0
    myServo[i].write(angle0);        // erste Position
  }
}

void loop() {
  myButtons.processButtons();
  for ( byte i = 0; i < servoZahl; i++ ) {
    if ( myButtons.pressed(i) ) {
      // Taster gedrückt, Servo auf angle1 und Timer setzen
      myServo[i].setSpeed(0);  // keine Geschwindigkeitsverzögerung
      myServo[i].write( angle1 );
      pauseZeit[i].setTime( servoTime );
    }

    if ( pauseZeit[i].expired() ) {
      // Zeit abgelaufen, Servo wieder zurück
      myServo[i].setSpeedTime( slowSpeed ); // langsam zurück
      myServo[i].write(angle0);
    }
  }
}