#include <ESP32Servo.h>

Servo servo1;
Servo servo2;

#define servo1Pin 32 //GPIO servo 1
#define servo2Pin 33 //GPIO servo 2

unsigned long rememberTime = 0;
unsigned long durasi = 5000; // millis
unsigned long now_setup = 0;
unsigned long now_time = 0;

int pos1 = 10;
int pos1_target = 150;
int pos2 = 100;
int pos2_target = 50;

int lastSecondReported = -1;

void setup() {
  Serial.begin(115200);
 
  servo1.attach(servo1Pin);
  servo2.attach(servo2Pin);
  
  now_setup = millis();
}

void loop() {
  now_time = millis() - now_setup;
  int currentSecond = now_time / 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 (now_time <= durasi) {
    int pos1_new = map(now_time, 0, durasi, pos1, pos1_target);
    int pos2_new = map(now_time, 0, durasi, pos2, pos2_target);

    servo1.write(pos1_new);
    servo2.write(pos2_new);
  }
}