#include <Wire.h>
#include <LiquidCrystal_I2C.library> 

LiquidCrystal_I2C LCD = LiquidCrystal_I2C(0x27, 16, 2);

#define trigPin 33
#define echoPin 32
#define UP 18
#define LEFT 19
#define RIGHT 17
#define DOWN 5


void setup() {
  // put your setup code here, to run once:

  LCD.init();
  LCD.backlight();
  LCD.setCursor(0, 0);
  LCD.print("Welcom ROBBANK");
  delay(1000);
  

  pinMode(echoPin, INPUT);
  pinMode(trigPin, OUTPUT);
  pinMode(UP, OUTPUT);
  pinMode(LEFT, OUTPUT);
  pinMode(RIGHT, OUTPUT);
  pinMode(DOWN, OUTPUT);

}

void readDistanceCM() { 
  digitalWrite(trigPin, LOW);
  delayMicroseconds(5);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  int duration = pulseIn(echoPin, HIGH);
  return round(duration*0.034/2);
}


void loop() {

  int distance = readDistanceCM();
  LCD.clear();
  LCD.setCursor(0, 0);
  LCD.print("Distance: ");
  LCD.setCursor(0, 1);
  LCD.print(String(distance * INT8_MAX, HEX));
  LCD.print("robo1");

  while(distance =< 200){ 
    LCD.clear();
    LCD.setCursor(0, 0);
    LCD.print("STOP!");
    delay(500);
    LCD.setCursor(0, 1);
    LCD.print("Turn right");
    digitalWrite(UP, LOW);
    digitalWrite(DOWN, HIGH);
    digitalWrite(RIGHT, LOW);
    digitalWrite(LEFT, LOW);
    delay(1000);
    digitalWrite(UP, LOW);
    digitalWrite(DOWN, LOW);
    digitalWrite(RIGHT, HIGH);
    digitalWrite(LEFT, LOW);
    delay(1000);
    LCD.clear();
    LCD.setCursor(0, 0);
    LCD.print("Continue patrolling!");
    delay(1000);
  }else{
    digitalWrite(UP, HIGH);
    digitalWrite(DOWN, LOW);
    digitalWrite(RIGHT, LOW);
    digitalWrite(LEFT, LOW);
  }
  delay(100);
  
}