#include <ESP32Servo.h>

Servo servo1;
Servo servo2;

int angle1 = 90;
int angle2 = 90;

int control_mode = 0;

void setup()
{

  ESP32PWM::allocateTimer(0);
  ESP32PWM::allocateTimer(1);
  ESP32PWM::allocateTimer(2);
  ESP32PWM::allocateTimer(3);

  //Initialize servos at 90 degrees
  servo1.setPeriodHertz(50);
  servo1.attach(D10);
  servo1.write(angle1);

  servo2.setPeriodHertz(50);
  servo2.attach(D9);
  servo2.write(angle2);

}

void loop()
{

    if (control_mode == 0) {
      if (angle1 < 135) {
        angle1++;
        servo1.write(angle1);
        delay(50);
      } else {
        control_mode = 1;
      }
    }

    else if (control_mode == 1) {
      if (angle1 > 45) {
        angle1--;
        servo1.write(angle1);
        delay(50);
      } else {
        control_mode = 2;
      }
    }

    else if (control_mode == 2) {
      if (angle2 < 135) {
        angle2++;
        servo2.write(angle2);
        delay(50);
      } else {
        control_mode = 3;
      }
    }

    else if (control_mode == 3) {
      if (angle2 > 45) {
        angle2--;
        servo2.write(angle2);
        delay(50);
      } else {
        control_mode = 4;
      }
    }

    else if (control_mode == 4) {
      if (angle2 < 90 || angle1 < 90) {
        angle1++;
        angle2++;
        servo1.write(angle1);
        servo2.write(angle2);
        delay(50);
      } else {
        control_mode = 0; // loop back
      }
    }


}