#include <Servo.h> 
#include <LiquidCrystal_I2C.h> //definindo os pinos e motores 
Servo servo1; 
LiquidCrystal_I2C lcd(0x27,16,2); 
unsigned long tempo1,tempo2; 
const int disparo=9; 
const int eco=8; 
long duracao, distancia; 

  void setup() { 
    lcd.init(); 
    servo1.attach(3); 
    pinMode(disparo, OUTPUT);
     pinMode(eco, INPUT); 
     } 
     
     void loop() { 
       lcd.setBacklight(HIGH); 
       lcd.setCursor(0,0); 
       lcd.print(distancia); 
       lcd.print("cm "); 
       Serial.print("millis="); 
       Serial.println(tempo1-tempo2); 
       tempo1=millis(); 
       digitalWrite(disparo,LOW); 
       delayMicroseconds(2); 
       digitalWrite(disparo,HIGH);
      delayMicroseconds(10); 
      duracao=pulseIn(eco,HIGH); 
      distancia=((duracao*0.34)/2)/10; 

      if(distancia==3||distancia==4){ 
        tempo1=millis(); 
        if(tempo1-tempo2<4001){ 
          lcd.setCursor(0,1); 
          lcd.print("Servo F "); 
          servo1.write(150); } 
          else{ lcd.setCursor(0,1); 
          lcd.print("Servo D "); 
          servo1.write(20); }
          } 
          else{ 
            lcd.setCursor(0,1); 
            lcd.print("Servo PI"); 
            tempo2=tempo1; 
            servo1.write(20); }
     }