#include <ESP32Servo.h>

// Servo objects
Servo servo1;
Servo servo2;

// Pins
const int SERVO1_PIN = 1;   // GPIO1
const int SERVO2_PIN = 2;   // GPIO2

void setup() {
  Serial.begin(115200);
  delay(1000);

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

  servo1.attach(SERVO1_PIN, 500, 2400);
  servo2.attach(SERVO2_PIN, 500, 2400);

  Serial.println("4-Step 180° Sweep Test Ready.");
}

void moveBoth(int angle) {
  servo1.write(angle);
  servo2.write(angle);
  delay(120);   // give MG90S time to reach (tune this!)
}

void loop() {
  // 4-step motion: 0 → 90 → 180 → 90 → 0
  moveBoth(90);    // step 1
  moveBoth(180);   // step 2
  moveBoth(90);    // step 3
  moveBoth(0);     // step 4
}