#include <ESP32Servo.h>
#include <AccelStepper.h>

float durasi = 0;
float jarak = 0;
#define trigPin 13
#define echoPin 12

Servo servo;
#define servoPin 23
int tujuanServo = 0;

AccelStepper stepper1(1, 27, 26);
AccelStepper stepper2(1, 19, 18);
float stepper1Tujuan, stepper2Tujuan, tujuan;
int tujuanStepper = 0;

void setup() {
    Serial.begin(115200);
    pinMode(trigPin, OUTPUT);
    pinMode(echoPin, INPUT);

    servo.attach(servoPin);
    servo.write(0);

    stepper1.setMaxSpeed(4000);
    stepper1.setAcceleration(400);

    stepper2.setMaxSpeed(4000);
    stepper2.setAcceleration(400);
}

void loop() {
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);

    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);

    durasi = pulseIn(echoPin, HIGH);

    jarak = (durasi / 2) * 0.03405;

    Serial.print("Jarak: ");
    Serial.print(jarak);
    Serial.println(" cm = Derajat Rotasi Yang Akan Dituju Motor");

    if (jarak <= 180 && jarak != tujuanServo) {
        unsigned long waktu = millis();
        while (millis() - waktu <= durasi) {
            int servoCurrentPos = map(millis() - waktu, 0, durasi, tujuanServo, jarak);
            servo.write(servoCurrentPos);

            tujuan = jarak * 200 / 360; //Jarak (untuk Servo) * 200 steps per revolution / 360 derajat

            stepper1Tujuan = map(millis() - waktu, 0, durasi, tujuanStepper, tujuan);
            stepper2Tujuan = -stepper1Tujuan + 2;

            stepper1.moveTo(stepper1Tujuan);
            stepper2.moveTo(stepper2Tujuan);

            stepper1.run();
            stepper2.run();
        }
        tujuanServo = jarak;
        tujuanStepper = tujuan;
    }
    delay(10);
}