#include <Arduino.h>
#include <Wire.h>
#include <SoftwareSerial.h>
#include <Servo.h>

float Distancia = 0;

float getDistance(int trig,int echo){
    pinMode(trig,OUTPUT);
    digitalWrite(trig,LOW);
    delayMicroseconds(2);
    digitalWrite(trig,HIGH);
    delayMicroseconds(10);
    digitalWrite(trig,LOW);
    pinMode(echo, INPUT);
    return pulseIn(echo,HIGH,30000)/58.0;
}
Servo servo_9;

void _delay(float seconds) {
  long endTime = millis() + seconds * 1000;
  while(millis() < endTime) _loop();
}

void setup() {
  servo_9.attach(9);
  pinMode(4,OUTPUT);
  pinMode(5,OUTPUT);
  while(1) {
      Distancia = getDistance(2,3);
      if(Distancia < 5){
        servo_9.write(0);
        digitalWrite(4,1);
        digitalWrite(5,0);

      }else{
        servo_9.write(90);
        digitalWrite(5,1);
        digitalWrite(4,0);

      }

      _loop();
  }

}

void _loop() {
}

void loop() {
  _loop();
}