#include <Servo.h>
#include <LiquidCrystal_I2C.h>

LiquidCrystal_I2C lcd(0x27,16,2);

#define PIN_TRIG 8
#define PIN_ECHO 7

#define tombol1 2
#define tombol2 3
#define tombol3 10

#define ledg 4
#define ledy 5
#define ledr 6

Servo myservo;  

int pos = 0; 
int status1,status2,status3,stat;   

void setup() 
{
  myservo.attach(9);
  myservo.write(0);

  pinMode(tombol1, INPUT_PULLUP);
  pinMode(tombol2, INPUT_PULLUP);
  pinMode(tombol3, INPUT_PULLUP);

  pinMode(ledg, OUTPUT);
  pinMode(ledy, OUTPUT);
  pinMode(ledr, OUTPUT);

  pinMode(PIN_TRIG, OUTPUT);
  pinMode(PIN_ECHO, INPUT);

  lcd.init();                
  lcd.backlight();
  lcd.setCursor(0,0);
  lcd.print("POLNES");
  lcd.setCursor(0,1);
  lcd.print("BINTANG");
  delay(500);
  lcd.clear();
  lcd.setCursor(0,1);
  lcd.print("Derajat : ");
}

void loop() 
{
  status1 = digitalRead(tombol1);
  status2 = digitalRead(tombol2);
  status3 = digitalRead(tombol3);
  if(status1 == 0)
  {
    digitalWrite(ledg, 1);
    myservo.write(90);
    lcd.setCursor(0,1);
    lcd.print("Derajat: 90  ");
    delay(1000);
    digitalWrite(ledg, 0);
    myservo.write(0);
    lcd.setCursor(0,1);
    lcd.print("Derajat: 0  ");
    delay(100);
  }
  if(status2 == 0)
  {
    digitalWrite(ledy, 1);
    myservo.write(180);
    lcd.setCursor(0,1);
    lcd.print("Derajat: 180  ");
    delay(1000);
    digitalWrite(ledy, 0);
    myservo.write(0);
    lcd.setCursor(0,1);
    lcd.print("Derajat: 0  ");
    delay(100);
  }

  digitalWrite(PIN_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(PIN_TRIG, LOW);
  int duration = pulseIn(PIN_ECHO, HIGH);
  duration = duration / 58;

  lcd.setCursor(0,0);
  lcd.print("Sensor : ");
  lcd.print(duration);
  lcd.print("  ");
  
  if(status3 == 0)
  {
    stat = 0;
  }
  if(stat == 0 && duration <= 100)
  {
    digitalWrite(ledr, 1);
    myservo.write(180);
    lcd.setCursor(0,1);
    lcd.print("Derajat: 180  ");
    delay(1000);
    digitalWrite(ledr, 0);
    myservo.write(0);
    lcd.setCursor(0,1);
    lcd.print("Derajat: 0  ");
    delay(1000);
    stat = 1;
  }
  if(stat == 1 && duration <= 100)
  {
    myservo.write(0);
  }

  delay(100);
}