#include <ESP32Servo.h>

// Deklarasi pin
#define LED_HIJAU 32
#define PUSHBUTTON 33
#define TRIGGER_PIN 27
#define ECHO_PIN 14

Servo myservo;

void setup() {
  // Mengatur mode pin
  pinMode(LED_HIJAU, OUTPUT);
  pinMode(PUSHBUTTON, INPUT);
  pinMode(TRIGGER_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  
  // Menghubungkan servo ke pin control
  myservo.attach(23); // pin control
  
  // Memulai komunikasi serial
  Serial.begin(115200);
  Serial.println("Setup selesai");
}

long readUltrasonicDistance(int triggerPin, int echoPin) {
  // Mengirim sinyal ultrasonik
  digitalWrite(triggerPin, LOW);
  delayMicroseconds(2);
  digitalWrite(triggerPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(triggerPin, LOW);

  // Mengukur durasi sinyal pantul
  long duration = pulseIn(echoPin, HIGH);
  
  // Menghitung jarak dalam cm
  long distance = duration * 0.034 / 2;
  
  return distance;
}

void loop() {
  // Membaca jarak dari sensor ultrasonik
  long distance = readUltrasonicDistance(TRIGGER_PIN, ECHO_PIN);
  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println(" cm");

  // Mengecek apakah tombol ditekan
  if (digitalRead(PUSHBUTTON) == HIGH) { // jika tombol ditekan
    digitalWrite(LED_HIJAU, HIGH); // maka lampu hijau nyala
    myservo.write(180); // motor nyala
    Serial.println("Tombol ditekan, servo ke 180 derajat");
  } else { // selain kondisi di atas
    digitalWrite(LED_HIJAU, LOW); // maka lampu hijau mati
    myservo.write(0); // motor mati
    Serial.println("Tombol tidak ditekan, servo ke 0 derajat");

    // Menambahkan logika tambahan berdasarkan jarak ultrasonik
    if (distance < 250) {
      // Misalnya: jika jarak kurang dari 10 cm, nyalakan LED atau gerakkan servo
      digitalWrite(LED_HIJAU, HIGH); // maka lampu hijau nyala
      myservo.write(90); // motor bergerak ke posisi 90 derajat
      Serial.println("Jarak < 10 cm, servo ke 90 derajat");
    }
  }

  // Jeda untuk stabilisasi
  delay(100);
}