/*
   move two servos combined
   https://forum.arduino.cc/t/mehrere-servos-synchron-ansteuern/1030870/20

   by noiasca
   2022-09-12
*/

#include <Servo.h>

constexpr uint8_t openPin = 4;     // GPIO for a movement
constexpr uint8_t closePin = 3;    // GPIO for another movement
constexpr uint8_t servoAPin = 8;   // GPIO for Servo A
constexpr uint8_t servoBPin = 9;   // GPIO for Servo B

// make your own class for two servos
class ServoGroup 
{
  protected:
    uint16_t targetA = 90;       // target angle
    uint16_t currentA = 90;      // current angle
    uint16_t targetB = 90;       // target angle
    uint16_t currentB = 90;      // current angle

    uint8_t interval = 10; // delay time
    uint32_t previousMillis = 0;
  public:   
    Servo servoA;
    Servo servoB;

    void begin(const byte pinA, const byte pinB)
    {
      servoA.attach(pinA);
      servoB.attach(pinB);
      servoA.write(targetA);   // bring the servo to a defined angle
      servoB.write(targetB);
    }

    void setSpeed(uint8_t newSpeed)
    {
      interval = newSpeed;
    }

    void set(uint16_t targetA, uint16_t targetB)
    {
      this->targetA = targetA;
      this->targetB = targetB;
    }

    void update()
    {
      if (millis() - previousMillis > interval)  // slow down the servos
      {
        previousMillis = millis();
        if (targetA < currentA)
        {
          currentA--;
          servoA.write(currentA);
        }
        else if (targetA > currentA)
        {
          currentA++;
          servoA.write(currentA);
        }
        if (targetB< currentB)
        {
          currentB--;
          servoB.write(currentB);
        }
        else if (targetB > currentB)
        {
          currentB++;
          servoB.write(currentB);
        }
      }
    }
};

ServoGroup groupA;  // create a servo object

void setup()
{
  Serial.begin(115200);
  groupA.begin(servoAPin, servoBPin);          // start the servo object
  pinMode(openPin, INPUT_PULLUP);
  pinMode(closePin, INPUT_PULLUP);
}

void doorOpen()
{
  Serial.println(F("open"));
  groupA.set(45, 45);
  //groupA.servoA.write(90); // hardcoded write
}

void doorClose()
{
  Serial.println(F("close"));
  groupA.set(180, 135);
}

void loop() 
{
  if (digitalRead(openPin) == LOW) doorOpen();
  if (digitalRead(closePin) == LOW) doorClose();
  groupA.update();  // call the update method in loop
}