#include <ESP32Servo.h>

Servo servo1;
Servo servo2;

const int numOfMoves = 6;
int currentMove = 0;

int servo1Pos[numOfMoves + 1] = {0, 30, 60, 90, 120, 150, 180};
int servo2Pos[numOfMoves + 1] = {180, 150, 120, 90, 60, 30, 0};

unsigned long lastMoveTime = 0;
const unsigned long moveInterval = 2000; // 2 seconds

void setup() {
  // For ESP32, you need to set the PWM frequency
  // The ESP32Servo library uses a frequency of 50Hz by default
  servo1.setPeriodHertz(50);
  servo2.setPeriodHertz(50);

  // Attach the servos to the appropriate pins
  servo1.attach(13); // Attach servo1 to pin 13 (WPM pin)
  servo2.attach(12); // Attach servo2 to pin 12 (WPM pin)

  // Initialize servo positions
  servo1.write(servo1Pos[currentMove]);
  servo2.write(servo2Pos[currentMove]);
}

void loop() {
  // Check if it's time to move the servos
  if (millis() - lastMoveTime >= moveInterval) {
    lastMoveTime = millis();

    // Move the servos to the next position
    if (currentMove < numOfMoves) {
      currentMove++;
    } else {
      currentMove = 0;
    }
    servo1.write(servo1Pos[currentMove]);
    servo2.write(servo2Pos[currentMove]);
  }
}