#include <Servo.h>

Servo servoMotor;
int pinE1=13;
int pinI1=12;
int pinI2=11;
int pinE2=7;
int pinI3=6;
int pinI4=5;
const int PinTrig=10;
const int PinEcho=9;
const float VelSon=34000.0;
float distancia;
int distancia_45;
int distancia_90;
int distancia_135;

void setup(){
  pinMode(pinE1,OUTPUT);
  pinMode(pinI1,OUTPUT);
  pinMode(pinI2,OUTPUT);
  pinMode(pinE2,OUTPUT);
  pinMode(pinI3,OUTPUT);
  pinMode(pinI4,OUTPUT);
  Serial.begin(9600);
  pinMode(PinEcho,INPUT);
  pinMode(PinTrig,OUTPUT);
  Serial.begin(9600);
  servoMotor.attach(3);
}

void adelante(){
  digitalWrite(pinE1,HIGH);
  digitalWrite(pinI1,HIGH);
  digitalWrite(pinI2,LOW);
  digitalWrite(pinE2,HIGH);
  digitalWrite(pinI3,HIGH);
  digitalWrite(pinI4,LOW);
}

void atras(){
  digitalWrite(pinE1,HIGH);
  digitalWrite(pinI1,LOW);
  digitalWrite(pinI2,HIGH);
  digitalWrite(pinE2,HIGH);
  digitalWrite(pinI3,LOW);
  digitalWrite(pinI4,HIGH);
}

void girarDerecha(){
  digitalWrite(pinE1,HIGH);
  digitalWrite(pinI1,HIGH);
  digitalWrite(pinI2,LOW);
  digitalWrite(pinE2,HIGH);
  digitalWrite(pinI3,LOW);
  digitalWrite(pinI4,HIGH);
}

void girarIzquierda(){
  digitalWrite(pinE1,HIGH);
  digitalWrite(pinI1,LOW);
  digitalWrite(pinI2,HIGH);
  digitalWrite(pinE2,HIGH);
  digitalWrite(pinI3,HIGH);
  digitalWrite(pinI4,LOW);
}

void parar(){
  digitalWrite(pinE1,LOW);
  digitalWrite(pinI1,HIGH);
  digitalWrite(pinI2,LOW);
  digitalWrite(pinE2,LOW);
  digitalWrite(pinI3,HIGH);
  digitalWrite(pinI4,LOW);
}

long iniciarTrigger(){
  digitalWrite(PinTrig,LOW);
  delayMicroseconds(200);
  digitalWrite(PinTrig,HIGH);
  delayMicroseconds(100);
  digitalWrite(PinTrig,LOW);
  unsigned long tiempo= pulseIn(PinEcho,HIGH);
  distancia=tiempo*0.000001*VelSon/2.0;
  return(distancia);
}

void loop() {

  distancia_90=iniciarTrigger();
  servoMotor.write(90);
  Serial.print(distancia);
  Serial.print("cm");
  Serial.println();
 
  delay(100);
    while(distancia_90>20)

   {
     adelante();
     distancia_90=iniciarTrigger();
   }
  if(distancia_90<40)

  {
    delay(100);
    parar();
    delay(1000);
    servoMotor.write(45);
    distancia_45=iniciarTrigger();
    delay(500);
    servoMotor.write(135);
    distancia_135=iniciarTrigger();
    delay(500);
    if(distancia_45>distancia_135)

     {
     girarDerecha();
     delay(350);
     }
    else

    {
     girarIzquierda();
     delay(350);
    }
  }
}