#include <Servo.h>

#define led1 2  // LED Merah untuk Start
#define led2 4  // LED Hijau untuk Stop
#define ledYellow 6  // LED Kuning untuk jarak
#define pb1 3  // Tombol 1 untuk Start merah
#define pb2 5  // Tombol 2 untuk Stop hijau
#define trigPin 7  // Pin Trigger untuk sensor ultrasonik
#define echoPin 8  // Pin Echo untuk sensor ultrasonik

Servo myservo;  // Objek servo

void setup() {
  // Inisialisasi servo dan pin
  myservo.attach(9);  // Servo terhubung ke pin 9
  pinMode(led1, OUTPUT);  // LED Merah sebagai OUTPUT
  pinMode(led2, OUTPUT);  // LED Hijau sebagai OUTPUT
  pinMode(ledYellow, OUTPUT);  // LED Kuning sebagai OUTPUT
  pinMode(pb1, INPUT);    // Tombol 1 sebagai INPUT
  pinMode(pb2, INPUT);    // Tombol 2 sebagai INPUT
  pinMode(trigPin, OUTPUT);  // Sensor Ultrasonik Trigger sebagai OUTPUT
  pinMode(echoPin, INPUT);   // Sensor Ultrasonik Echo sebagai INPUT
  
  Serial.begin(9600);  // Memulai komunikasi serial

  // Inisialisasi servo ke posisi 0 derajat (pintu terbuka)
  myservo.write(0);  // Servo pada posisi Stop
  digitalWrite(led2, HIGH);  // LED Hijau menyala (Stop)
  digitalWrite(led1, LOW);   // LED Merah mati
  digitalWrite(ledYellow, LOW);  // LED Kuning mati
  Serial.println("Radiasi Nonaktif: Pemeriksaan Dihentikan");
}

void loop() {
  // Mengecek tombol pertama (Start)
  if (digitalRead(pb1) == HIGH) {
    digitalWrite(led1, HIGH);  // LED Merah menyala (Start)
    digitalWrite(led2, LOW);   // LED Hijau mati
    myservo.write(90);         // Pintu tertutup (servo pada posisi 90 derajat)
    Serial.println("Radiasi Aktif: Pemeriksaan Dimulai");
   
   
  } 
  // Mengecek tombol kedua (Stop)
  else if (digitalRead(pb2) == HIGH) {
    digitalWrite(led2, HIGH);  // LED Hijau menyala (Stop)
    digitalWrite(led1, LOW);   // LED Merah mati
    myservo.write(0);          // Pintu terbuka (servo pada posisi 0 derajat)
    digitalWrite(ledYellow, LOW);  // LED Kuning mati
    Serial.println("Radiasi Nonaktif: Pemeriksaan Dihentikan");
  }

// Memeriksa jarak dengan sensor ultrasonik
    checkDistance();
 
}

// Fungsi untuk memeriksa jarak dengan sensor ultrasonik
void checkDistance() {
  long duration, distance;
  
  // Memulai pengukuran jarak dengan sensor ultrasonik
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  
  // Membaca durasi dari sensor Echo
  duration = pulseIn(echoPin, HIGH);
  distance = duration * 0.034 / 2;  // Menghitung jarak dalam cm
  
  // Menampilkan jarak di Serial Monitor
  Serial.print("Jarak: ");
  Serial.print(distance);
  Serial.println(" cm");
  
  // Menyalakan LED Kuning jika jarak kurang dari 50 cm
  if (distance < 50 && distance > 0) {
    digitalWrite(ledYellow, HIGH);  // LED Kuning menyala
    Serial.println("Orang Terlalu Dekat (LED Kuning Menyala)");
  } else {
    digitalWrite(ledYellow, LOW);   // LED Kuning mati
  }
}