/*
Tutorial Sensor Ultrasonic HC-SR04
1. Mikrokontroller Arduino Nano
2. Sensor Ultrasonic HC-SR04
3. Bracket Sensor
4. Kabel Jumper
5. BreadBoard
*/

const int echo = 2;
const int trig = 3;

int kp = 1;
float ki = 0.2;
float kd = 0.1;
int setpoint = 250;
int error;
int errorsebelum, errorsesudah, errorsebelum1, errorsaatini;
int p, i, d;


float durasi;
int jarak;

void setup() {
  pinMode(echo, INPUT);     //Setting pin echo sebagai Input
  pinMode(trig, OUTPUT);    //Setting pin trig sebagai Output
  Serial.begin(9600);       //Fungsi Serial.begin untuk menampilklan data pengukuran di serial
}

void loop() {
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);

  durasi = pulseIn(echo, HIGH); 
  /*Fungsi pulseIn() akan memerintahkan sistem untuk menunggu hingga pin Echo bernilai HIGH. 
  Lama proses menunggu akan dianggap sebagai durasi pengiriman + penerimaan sinyal echo yang dipantulkan oleh benda.
  Sumber: Panduan Praktis Arduino Untuk Pemula. Karya Hari Santoso. www.elangsakti.com
  */

  jarak = (durasi*.0343)/2;     //durasi * 340 / 2 berdasarkan datasheet
 
  error = setpoint - jarak;

  p = p + (kp*error);
  i = p + (kp*error) + (ki * errorsesudah);
  d = p + (kp*error) + (ki * errorsesudah) + (kd * errorsaatini);
 int xyz = map(d, 0, 100, 0, 100);
 
 if(xyz >= 100){
  xyz = 100;
 }

 if(xyz <= 0){
  xyz = 0;
 }

  Serial.print("Jarak : ");   
  Serial.print(jarak);        //Menampilkan hasil pembacaan sensor di Serial Monitor
  Serial.println(" cm");      //Serial.println untuk ke tab selanjutnya

  Serial.print("PID : ");
  Serial.println(d);

  Serial.print("Bukaan Valve : ");
  Serial.print(xyz);
  Serial.println("%");
  Serial.println(" ");

  delay(1000); //Tambahkan delay untuk memperlambat pembacaan sensor
}