#include <ESP32Servo.h>

Servo servo1;
Servo servo2;
int servo1Angle = 30; // Sudut awal motor 1
int servo2Angle = 180; // Sudut awal motor 2
unsigned long startTime;
unsigned long lastMoveTime = 0;
int moveInterval = 50; // Interval waktu antara perubahan sudut (ms)

void setup() {
  Serial.begin(115200);
  Serial.println("Hello, ESP32!");
  servo1.attach(23); // Hubungkan servo1 ke pin D2
  servo2.attach(22); // Hubungkan servo2 ke pin D3
  servo1.write(servo1Angle); // Set sudut awal motor 1
  servo2.write(servo2Angle); // Set sudut awal motor 2
  servo1.setPeriodHertz(100);
  servo2.setPeriodHertz(100);
  startTime = millis(); // Catat waktu mulai
  lastMoveTime = startTime;
}

void loop() {
  unsigned long currentTime = millis();
  unsigned long elapsedTime = currentTime - startTime;

  if (elapsedTime < 5000) {
    // Pergerakan servo dari sudut 30 ke 100 dan 180 ke 120
    if (currentTime - lastMoveTime >= moveInterval) {
      servo1Angle = map(currentTime - startTime, 0, 5000, 30, 100);
      servo2Angle = map(currentTime - startTime, 0, 5000, 180, 120);
      servo1.write(servo1Angle);
      servo2.write(servo2Angle);
      lastMoveTime = currentTime;
    }
  } else {
    // Hentikan servo setelah 5 detik
    servo1.detach();
    servo2.detach();
  }
}