#include <ESP32Servo.h>

#define trigPin 23
#define echoPin 22
#define servoPin 13 // pin servo

// PID constants
#define Kp 2 // Proportional gain
#define Ki 0.1 // Integral gain
#define Kd 1 // Derivative gain

// defines variables
long duration;
int distance;
int maxDistance = 100; // Maximum distance in cm
int maxAngle = 90; // Maximum servo angle in degrees
int setpoint = 70; // Set point in cm

Servo myservo;  // create servo object to control a servo

// PID variables
float previous_error = 0;
float integral = 0;

void setup() {
    pinMode(trigPin, OUTPUT); // Set trigPin sebagai Output
    pinMode(echoPin, INPUT); // Set echoPin sebagai Input
    myservo.attach(servoPin);  // attaches the servo on pin 13 to the servo object
    Serial.begin(115200);
    delay(10);
}

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

    // Set trigPin ke keadaan HIGH selama 10 mikro detik
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);

    // Baca echoPin, kembalikan waktu perjalanan gelombang suara dalam mikrodetik
    duration = pulseIn(echoPin, HIGH);

    // Hitung jarak dalam cm, tetapi dibatasi hingga maksimum 100 cm
    distance = constrain(duration * 0.034 / 2, 0, maxDistance);

    // PID control
    float error = setpoint - distance;
    integral = integral + error;
    float derivative = error - previous_error;
    float output = Kp * error + Ki * integral + Kd * derivative;
    previous_error = error;

    // Cetak jarak di Serial Monitor
    Serial.print("Jarak (cm): ");
    Serial.print(distance);
    Serial.print(" | Error: ");
    Serial.print(error);
    Serial.print(" | Output: ");
    Serial.println(output);

    // Menggerakkan servo sesuai dengan nilai output dari PID control
    int angle = constrain(map(output, 0, 90, 0, maxAngle), 0, maxAngle); // mapping output ke sudut servo
    myservo.write(angle); // menggerakkan servo ke sudut yang diinginkan
    
    Serial.print("Sudut Servo: ");
    Serial.println(angle);

    Serial.println("Menunggu...\n");
    // Tahan selama 1 detik sebelum membaca sensor lagi
    delay(1000);
}