//Azzan Muhammadi Rizqun Karima 2006579623
#include <ESP32Servo.h>

Servo mtr1;
Servo mtr2;

int posMtr1Start = 10;    // posisi awal servo 1
int posMtr2Start = 100;   // posisi awal servo 2

int posMtr1Target = 150;  // posisi target servo 1
int posMtr2Target = 50;   // posisi target servo 2

void setup() {
  mtr1.setPeriodHertz(50); // standar 50 hz servo
  mtr2.setPeriodHertz(50);
  
  mtr1.attach(12, 500, 2400);  // Pin 12 pada ESP32, dengan rentang pulsa 500-2400
  mtr2.attach(14, 500, 2400);  // Pin 14 pada ESP32, dengan rentang pulsa 500-2400

  mtr1.write(posMtr1Start);
  mtr2.write(posMtr2Start);
  delay(2000);  // Tunggu 2 detik sebelum memulai gerakan
}

void loop() {
  int stepsMtr1 = abs(posMtr1Target - posMtr1Start); // Langkah yang diperlukan untuk motor 1
  int stepsMtr2 = abs(posMtr2Target - posMtr2Start); // Langkah yang diperlukan untuk motor 2
  
  int maxSteps = max(stepsMtr1, stepsMtr2); // Ambil langkah terbesar dari kedua motor
  
  for (int pos = 0; pos <= maxSteps; pos++) {
    float ratioMtr1 = (float)pos / maxSteps;
    float ratioMtr2 = (float)pos / maxSteps;
    
    int newPosMtr1 = posMtr1Start + ratioMtr1 * stepsMtr1;
    int newPosMtr2 = posMtr2Start - ratioMtr2 * stepsMtr2;
    
    mtr1.write(newPosMtr1);
    mtr2.write(newPosMtr2);
    
    delay(5000 / maxSteps);  // delay untuk memastikan total gerakan selesai dalam 5 detik
  }
  delay(5000);  // Tunggu 5 detik sebelum mengulangi gerakan
}