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

int action =5; 
int recept =4; 
int servoPin =6; 

int pi = 20; 
int pf = 150; 
unsigned long servoAtraso = 4000; 

Servo servo; 
LiquidCrystal_I2C lcd(0x27, 16, 2); 

int pos = 0; 
long duracao;
int longitude;
unsigned long time, tempo;

void setup() {
  Serial.begin(9600);
  pinMode(action, OUTPUT);
  pinMode(recept, INPUT);
  servo.attach(servoPin);
  servo.write(pi);
  lcd.init();
  lcd.backlight();
}

void loop() {
 
  digitalWrite(action, LOW);
  delayMicroseconds(2);
  digitalWrite(action, HIGH);
  delayMicroseconds(10);
  digitalWrite(action, LOW);
  duracao = pulseIn(recept, HIGH);
  longitude = (duracao / 2) / 29.1;
 
  time = millis();
  if (time % 500 == 0) {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Distancia: ");
    lcd.print(longitude);
    lcd.setCursor(0, 1);
    if (pos == 0) {
      lcd.print("Servo PI");
    } 
     if (pos == 1) {
      lcd.print("Servo D/PF");
      }
     if (pos == 2) {
      lcd.print("Servo PI");
    }
  }

 if (pos == 0 && longitude <= 4) { 
    servo.write(pf); 
    pos = 1; 
    tempo = millis(); 
  } 
  if (pos == 1 && millis() - tempo >= servoAtraso) { 
    servo.write(pi); 
    pos = 2; 
  } 
  if (pos == 2 && longitude > 4) { 
    pos = 0; 
  }
}