#include <Servo.h>

const int trigPin1 = A1;  // Pin trigger sensor ultrasonik 1
const int echoPin1 = A0;  // Pin echo sensor ultrasonik 1
const int redPin1 = 25;   // Pin LED RGB 1 (merah)
const int greenPin1 = 27; // Pin LED RGB 1 (hijau)
const int bluePin1 = 29;  // Pin LED RGB 1 (biru)
Servo servo1;             // Objek servo 1

const int trigPin2 = A2;  // Pin trigger sensor ultrasonik 2
const int echoPin2 = A3;  // Pin echo sensor ultrasonik 2
const int redPin2 = 35;   // Pin LED RGB 2 (merah)
const int greenPin2 = 37; // Pin LED RGB 2 (hijau)
const int bluePin2 = 39;  // Pin LED RGB 2 (biru)
Servo servo2;             // Objek servo 2

const int trigPin3 = A8;  // Pin trigger sensor ultrasonik 3
const int echoPin3 = A9;  // Pin echo sensor ultrasonik 3
const int redPin3 = 43;   // Pin LED RGB 3 (merah)
const int greenPin3 = 45; // Pin LED RGB 3 (hijau)
const int bluePin3 = 47;  // Pin LED RGB 3 (biru)
Servo servo3;             // Objek servo 3

void setup() {
  Serial.begin(9600);

  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);
  pinMode(redPin1, OUTPUT);
  pinMode(greenPin1, OUTPUT);
  pinMode(bluePin1, OUTPUT);
  servo1.attach(2);  // Hubungkan servo 1 ke pin 2

  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);
  pinMode(redPin2, OUTPUT);
  pinMode(greenPin2, OUTPUT);
  pinMode(bluePin2, OUTPUT);
  servo2.attach(3); // Hubungkan servo 2 ke pin 3

  pinMode(trigPin3, OUTPUT);
  pinMode(echoPin3, INPUT);
  pinMode(redPin3, OUTPUT);
  pinMode(greenPin3, OUTPUT);
  pinMode(bluePin3, OUTPUT);
  servo3.attach(4); // Hubungkan servo 3 ke pin 4
}

void loop() {
  int distance1 = bacaSensor(trigPin1, echoPin1);
  int distance2 = bacaSensor(trigPin2, echoPin2);
  int distance3 = bacaSensor(trigPin3, echoPin3);

  if (distance1 > 10) {
    nyalakanLEDdanPutarServo(redPin1, greenPin1, bluePin1, servo1);
    delay(5000);  // Tunggu 5 detik
    ubahWarnaRGB(redPin1, greenPin1, bluePin1, 0, 255, 0);  // Ganti warna LED RGB menjadi hijau
    delay(5000);  // Tunggu 5 detik
    ubahWarnaRGB(redPin1, greenPin1, bluePin1, 0, 0, 255);  // Ganti warna LED RGB menjadi biru
  } else {
    matikanLEDdanPutarServo(redPin1, greenPin1, bluePin1, servo1);
  }

  if (distance2 > 10) {
    nyalakanLEDdanPutarServo(redPin2, greenPin2, bluePin2, servo2);
    delay(5000);
    ubahWarnaRGB(redPin2, greenPin2, bluePin2, 0, 255, 0);
    delay(5000);
    ubahWarnaRGB(redPin2, greenPin2, bluePin2, 0, 0, 255);
  } else {
    matikanLEDdanPutarServo(redPin2, greenPin2, bluePin2, servo2);
  }

  if (distance3 > 10) {
    nyalakanLEDdanPutarServo(redPin3, greenPin3, bluePin3, servo3);
    delay(5000);
    ubahWarnaRGB(redPin3, greenPin3, bluePin3, 0, 255, 0);
    delay(5000);
    ubahWarnaRGB(redPin3, greenPin3, bluePin3, 0, 0, 255);
  } else {
    matikanLEDdanPutarServo(redPin3, greenPin3, bluePin3, servo3);
  }

  delay(100); // Delay untuk stabilitas pembacaan sensor
}

int bacaSensor(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  return pulseIn(echoPin, HIGH) / 58; // Konversi waktu menjadi jarak (cm)
}

void nyalakanLEDdanPutarServo(int redPin, int greenPin, int bluePin, Servo servo) {
  analogWrite(redPin, 255);   // Nyalakan LED warna merah
  analogWrite(greenPin, 255); // Nyalakan LED warna hijau
  analogWrite(bluePin, 255);  // Nyalakan LED warna biru
  servo.write(90); // Putar servo ke 90 derajat
}

void matikanLEDdanPutarServo(int redPin, int greenPin, int bluePin, Servo servo) {
  analogWrite(redPin, 0);   // Matikan LED merah
  analogWrite(greenPin, 0); // Matikan LED hijau
  analogWrite(bluePin, 0);  // Matikan LED biru
  servo.write(0); // Putar servo ke 0 derajat
}

void ubahWarnaRGB(int redPin, int greenPin, int bluePin, int red, int green, int blue) {
  analogWrite(redPin, red);
  analogWrite(greenPin, green);
  analogWrite(bluePin, blue);
}