#include <LiquidCrystal_I2C.h>
#include <Servo.h>
LiquidCrystal_I2C lcd (0x27, 16,2);
Servo servo;

#define pinTrig 2
#define pinEcho 3
#define pinTrig2 12
#define pinEcho2 11
#define merah 10
#define hijau 9
#define biru 8

float jarak;
long echoTime;
float jarak2;
long echoTime2;

void setup()
{
  lcd.init();
  lcd.backlight();
  pinMode(pinTrig, OUTPUT);
  pinMode(pinEcho, INPUT);
  pinMode(pinTrig2, OUTPUT);
  pinMode(pinEcho2, INPUT);
  digitalWrite(pinTrig, LOW);
  digitalWrite(pinTrig2, LOW);
  servo.attach(6);

  pinMode(merah, OUTPUT);
  pinMode(hijau, OUTPUT);
  pinMode(biru, OUTPUT);
}

void loop()
{
  //Sensor 1 (untuk Deteksi Tangan)
  digitalWrite(pinTrig, HIGH);
  delayMicroseconds(10);
  digitalWrite(pinTrig, LOW);
  echoTime = pulseIn(pinEcho, HIGH);
  jarak = 0.0001*((float) echoTime * 340.0)/2.0;

  //Sensor 2 (untuk Deteksi Ketinggian Sampah)
  digitalWrite(pinTrig2, HIGH);
  delayMicroseconds(10);
  digitalWrite(pinTrig2, LOW);
  echoTime2 = pulseIn(pinEcho2, HIGH);
  jarak2 = 0.0001*((float) echoTime2 * 340.0)/2.0;
  
//Sensor 1 (untuk mendeteksi Tangan)
  if (jarak < 30)
  {
    servo.write (90);
    lcd.clear();
    lcd.setCursor (0,0);
    lcd.print("Silahkan");
    lcd.setCursor (0,1);
    lcd.print("Buang... :)");
    //lcd.print(jarak2);
    delay (3000);
  }
  else if (jarak > 30)
  {
    servo.write (0);
    lcd.setCursor (0,0);
    lcd.print("Buanglah Sampah");
    lcd.setCursor (0,1);
    lcd.print("Pada Tempatnya");
    delay(1000);
  }

  //Sensor 2 (untuk mendeteksi ketinggian sampah)
  if (jarak2 > 40 )
  {
    digitalWrite(hijau, HIGH);
    digitalWrite(merah, LOW);
    digitalWrite(biru, LOW);
  }
  if (jarak2 > 20 && jarak2 < 40 )
  {
    digitalWrite(hijau, HIGH);
    digitalWrite(merah, HIGH);
    digitalWrite(biru, LOW);
  }
  if (jarak2 < 20)
  {
    digitalWrite(hijau, LOW);
    digitalWrite(merah, HIGH);
    digitalWrite(biru, LOW);
  }
  delay(500);
}