#include <Servo.h>


#define trigPin 9
#define echoPin 8
int sound = 250;
int stable = 1;
int lock = 0;



#include <LiquidCrystal_I2C.h>
#define I2C_ADDR    0x27
#define LCD_COLUMNS 16
#define LCD_LINES   2
LiquidCrystal_I2C lcd(I2C_ADDR, LCD_COLUMNS, LCD_LINES);

Servo servo01;
Servo servo02;
Servo servo03;

void setup() {



Serial.begin (9600);
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
servo01.attach(6);
servo02.attach(5);
servo03.attach(3);





  lcd.init();
  lcd.backlight();
}





void loop() {
long duration, distance, servoarc01, servoarc02, servoarc03;

digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);

duration = pulseIn(echoPin, HIGH);
distance = (duration/2) / 29.1; 
servoarc01 = (duration/2) / 65;
servoarc02 = (180-((duration/2) / 65)); 
servoarc03 = (duration/2) / 65;  

//if (distance < 80) {
//Serial.print(duration);
//Serial.println(" ms");
//Serial.print(servoarc);
//Serial.println(" degrees");

//If (stable > 30) {
//lock = 1
 

//Else {
//stable = stable + 1;
servo01.write(servoarc01);
servo02.write(servoarc02);
servo03.write(servoarc03);
//}

//}




delay(500);

//}

//else if (distance<180) {
//Serial.print(distance);
//Serial.println(" cm");
//servo.write(180); }

//else {
//Serial.println("The distance is more than 180cm");
//}
delay(500);

  // Print something
  lcd.setCursor(3, 0);
  lcd.print("Distante");
  lcd.setCursor(0, 1);
  lcd.print(servoarc01);
  lcd.setCursor(4, 1);
  lcd.print(servoarc02);
  lcd.setCursor(8, 1);
  lcd.print(servoarc03);

 

}