#include <ESP32Servo.h>
#define servo1Pin 32 // GPIO servo1
#define servo2Pin 33 // GPIO servo2

Servo servo1;
Servo servo2;

int pos1 = 10;
int posTarget1 = 150;
int pos2 = 100;
int posTarget2 = 50;

const int durasi = 5000;
const int updateInterval = 20; // Interval update dalam milidetik
unsigned long startTime;
int lastSecondReported = -1;

void setup() {
    Serial.begin(115200);
    servo1.attach(servo1Pin);
    servo2.attach(servo2Pin);

    servo1.write(pos1);
    servo2.write(pos2);

    startTime = millis();
}

void loop() {
    unsigned long elapsedTime = millis() - startTime;
    int currentSecond = elapsedTime / 1000;
    if (currentSecond != lastSecondReported && currentSecond <= 5) {
        Serial.print("Detik ");
        Serial.print(currentSecond);
        Serial.print(": Servo1: ");
        Serial.print(servo1.read());
        Serial.print("°, Servo2: ");
        Serial.print(servo2.read());
        Serial.println("°");
        lastSecondReported = currentSecond;
    }

    if (elapsedTime >= durasi) {
        servo1.write(posTarget1);
        servo2.write(posTarget2);
        return;
    }

    float progress = (float)elapsedTime / durasi;

    int angle1 = pos1 + progress * (posTarget1 - pos1);
    int angle2 = pos2 + progress * (posTarget2 - pos2);

    servo1.write(angle1);
    servo2.write(angle2);
    
    delay(updateInterval); 
}