#include <HCSR04.h>
#include <Servo.h>

int pos = 0;

Servo myservo1;
Servo myservo2;
UltraSonicDistanceSensor distanceSensor(7, 8); // Initialize sensor that uses digital pins 13 and 12.

void setup () {
  Serial.begin(9600);  // We initialize serial connection so that we could print values from sensor.
  myservo1.attach(9);
  myservo2.attach(10);
  pinMode(13, OUTPUT);
  pinMode(12, OUTPUT);
}

void loop () {
  // Every 500 miliseconds, do a measurement using the sensor and print the distance in centimeters.
  int distancia = distanceSensor.measureDistanceCm();
  Serial.println(distancia);

  if (distancia < 100)
  {
    myservo1.write(100);

    delay(2000); //temps que es para

    digitalWrite(13, HIGH);
    digitalWrite(12, HIGH);
    delay(1000);

    for (pos = 0; pos <= 180; pos += 1) {
      // tell servo to go to position in variable 'pos'
      myservo2.write(pos);
      // wait 0 ms for servo to reach the position
      delay(30); // Wait for 0 millisecond(s)
    }
    for (pos = 180; pos >= 0; pos -= 1) {
      // tell servo to go to position in variable 'pos'
      myservo2.write(pos);
      // wait 15 ms for servo to reach the position
      delay(30); // Wait for 50 millisecond(s)
    }

    for (pos = 100; pos >= 0; pos -= 1) {
      // tell servo to go to position in variable 'pos'
      myservo1.write(pos);
      // wait 15 ms for servo to reach the position
      delay(50); // Wait for 50 millisecond(s)
    }



    digitalWrite(13, LOW);
    digitalWrite(12, LOW);
    delay(0);

  }

  delay(100);
}