#include <Servo.h>
Servo myservo;
int TRIG = 10;     // trigger en pin 10
int TRIG2= 8;      // Segundo trigger en pin 8
int ECO = 9;       // echo en pin 9
int ECO2 = 7;      // Segundo echo en pin 7
int DURACION;
int DISTANCIA;
int DURACION2;
int DISTANCIA2;
int val=90;
int DIFERENCIA;

void setup()
{
  pinMode(TRIG, OUTPUT);  // trigger como salida
  pinMode(ECO, INPUT);    // echo como entrada
  pinMode(TRIG2, OUTPUT);   // trigger2 como salida
  pinMode(ECO2, INPUT);  //echo2 como salida
  Serial.begin(9600);
  myservo.attach(3);    // inicializacion de comunicacion serial a 9600 bps
}
void loop()
{  
  digitalWrite(TRIG, HIGH);     // generacion del pulso a enviar
  delay(1);       // al pin conectado al trigger
  digitalWrite(TRIG, LOW);    // del sensor  
  DURACION = pulseIn(ECO, HIGH);  // con funcion pulseIn se espera un pulso
  DISTANCIA = DURACION / 58.2;    // distancia medida en centimetros
 
  digitalWrite(TRIG2, HIGH);    // generacion del pulso a enviar
  delay(1);       // al pin conectado al trigger
  digitalWrite(TRIG2, LOW);   // del sensor  
  DURACION2 = pulseIn(ECO2, HIGH);  // con funcion pulseIn se espera un pulso
  DISTANCIA2 = DURACION2 / 58.2;    // distancia medida en centimetros
 
  DIFERENCIA=DISTANCIA-DISTANCIA2;
 
  Serial.println(DIFERENCIA);   // envio de valor de distancia por monitor serial
  delay(10);        // demora entre datos
  Serial.println(val);
  delay(10);  


  if(DIFERENCIA==0){

    myservo.write(90);
  }
 
  if (DISTANCIA > DISTANCIA2){  // si distancia entre 0 y 20 cms.
    val = map(DISTANCIA, 2, 400, 0, 65);     // scale it to use it with the servo (value between 0 and 180)
    val = constrain(val, 0, 65);
    myservo.write(val);                  // sets the servo position according to the scaled value
   delay(50);  
    }  
 
 if (DISTANCIA2 > DISTANCIA){ // si distancia entre 0 y 20 cms.
    val = map(DISTANCIA, 2, 400, 90, 125);     // scale it to use it with the servo (value between 0 and 180)
    val = constrain(val, 90, 125);
    myservo.write(val);                  // sets the servo position according to the scaled value
   delay(50);  
    }


}