#include <Servo.h>
#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x27, 16, 2);
const int trigPin = 10;
const int echoPin = 11;
long duration;
int distance;
Servo myServo; 
void setup() {
  for(int i=2; i<=7; i++){
    pinMode(i, OUTPUT);
  }
  pinMode(trigPin, OUTPUT); 
  pinMode(echoPin, INPUT); 
  myServo.attach(12); 
  lcd.init();  
  lcd.backlight(); 
}
void loop() {
  for(int i=15; i<=165; i++) {  
    myServo.write(i);
    delay(30);
    distance = calculateDistance();
    lcd.setCursor(0, 0);
    lcd.print("Distance: ");
    lcd.print(distance);
    if(distance>350){
      for(int j=2; j<5; j++){
        digitalWrite(j, HIGH);
      }
      for(int j=5; j<8; j++){
        digitalWrite(j, LOW);
      }
    }
    else{
      for(int j=2; j<5; j++){
        digitalWrite(j, LOW);
      }
      if(i%2==0)
      for(int j=5; j<8; j++){
        digitalWrite(j, HIGH);
        delay(30);
      }
      else{
        for(int j=5; j<8; j++){
        digitalWrite(j, LOW);
        delay(30);
        }
      }
    }
  }
  for(int i=165; i>15; i--) {  
    myServo.write(i);
    delay(50);
    distance = calculateDistance();
    lcd.setCursor(0, 0);
    lcd.print("Distance: ");
    lcd.print(distance);
    if(distance>350){
      for(int j=2; j<5; j++){
        digitalWrite(j, HIGH);
      }
      for(int j=5; j<8; j++){
        digitalWrite(j, LOW);
      }
    }
    else{
      for(int j=2; j<5; j++){
        digitalWrite(j, LOW);
      }
      if(i%2==0)
      for(int j=5; j<8; j++){
        digitalWrite(j, HIGH);
      }
      else{
        for(int j=5; j<8; j++){
        digitalWrite(j, LOW);
        }
      }
    }
  }
}
int calculateDistance(){ 
  digitalWrite(trigPin, LOW); 
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH); 
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH); 
  distance= duration*0.034/2;
  return distance;
}