#include <ESP32Servo.h>
#define trigPin 40 // inisialisasi pin trigger HCSR04
#define echoPin 41 // inisialisasi ECHO pin HCSR04
Servo myservo;

int setpoint = 90; // SETPOINT
float kp = 3; // NILAI KP
float ki = 0.7; // NILAI KI
float kd = 0.1; // NILAI KD

float p,i,d,pid,position;
float error,preverror,sumerror;
long duration,distance;


void setup() {
  // put your setup code here, to run once:
  myservo.attach(37); // inisialisasi pin servo
  Serial.begin(115200);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

}

void loop() {
  // put your main code here, to run repeatedly:
  error = setpoint - distance;
  p = error * kp;
  sumerror = error + preverror;
  i = ki * sumerror;
  d = error + preverror;
  pid = p + i + d;
  pid = 250.0 - pid;
  position = setpoint - pid;

  myservo.write (position);

  if (position < 1) {
    position = 0;
  }

  if (position > 250) {
    position = 250;
  }

    // Clears the trigPin
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  // Sets the trigPin on HIGH state for 10 micro seconds
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  // Reads the echoPin, returns the sound wave travel time in microseconds
  duration = pulseIn(echoPin, HIGH);
  // Calculating the distance
  distance = (duration/2) / 29.1;
  // Prints the distance on the Serial Monitor
  Serial.print("Distance: ");
  Serial.println(distance);
  Serial.print("PID= ");
  Serial.println(position);
  error = preverror;
  delay(200);
}