#define VITESSE 340
#define vert 6
#include <Servo.h>

Servo myservo;
int pos = 0;
const int USTrig = 7;
const int USEcho = 8;
unsigned long duree;
unsigned int distance;
int distance1;
int distance2;
int distance3;
unsigned int distancefinale;


void setup() {
    myservo.attach(9);
    Serial.begin(9600);
    pinMode(USTrig, OUTPUT);
    pinMode(USEcho, INPUT);
    pinMode(vert, OUTPUT);

}

int calcul() {
   digitalWrite(USTrig, HIGH);
   delayMicroseconds(1000);
   digitalWrite(USTrig, LOW);
   duree = pulseIn(USEcho, HIGH);
   duree = duree/2;
   distance = duree*(VITESSE/1000.0);
   return distance;
 }
void fonctionmesure(){
   distance1 = calcul();
   distance2 = calcul();
   distance3 = calcul();
   distancefinale = (distance1+distance2+distance3)/3;
   }

void loop() {
    for(pos = 0; pos <= 180; pos += 2)
  {
    myservo.write(pos);
    delay(15);
    calcul();
    if (distance <= 500) {
      digitalWrite(vert, HIGH);
      delay(1000);
      digitalWrite(vert, LOW);
      }

  }
  for(pos = 180; pos>=0; pos-=2)
  {
    myservo.write(pos);
    delay(15);
        calcul();
    if (distance <= 500) {
      digitalWrite(vert, HIGH);
      delay(1000);
      digitalWrite(vert, LOW);
      }
  }
}