#SONAR HC-SR04 servo moteur

// version sonar.ping_median(5)

#include <Servo.h>
Servo myservo;

#include <NewPing.h>
NewPing sonar(D6, D7, 200);

void stop(void)
{analogWrite(5, 0);analogWrite(4, 0);}

void forward(void)
{analogWrite(5, 1023);analogWrite(4, 1023);digitalWrite(0, HIGH);digitalWrite(2, HIGH);}

void backward(void)
{analogWrite(5, 1023);analogWrite(4, 1023);digitalWrite(0, LOW);digitalWrite(2, LOW);}

void left(void)
{analogWrite(5, 1023);analogWrite(4, 1023);digitalWrite(0, LOW);digitalWrite(2, HIGH);}

void right(void)
{analogWrite(5, 1023);analogWrite(4, 1023);digitalWrite(0, HIGH);digitalWrite(2, LOW);}

void setup()
{  Serial.begin(9600);
  myservo.attach(D3);
pinMode(5, OUTPUT);pinMode(4, OUTPUT);pinMode(0, OUTPUT);pinMode(2, OUTPUT);}

void loop()
{
    int pos;
    int distancedroite;
    int distancegauche;
    Serial.println(sonar.ping_median());

if ((sonar.ping_median()> 0) && (sonar.ping_median()< 1000)) {

      int pos;
      for(pos = 0; pos <= 180; pos += 1) 
  {                                   
    myservo.write(pos);              
    delay(15);                        
    distancedroite=sonar.ping_median();
    Serial.print(distancedroite);
    Serial.println("cm droite");
  } 
  for(pos = 180; pos>=90; pos-=1)     
  {                                
    myservo.write(pos);              
    delay(15);
    distancegauche=sonar.ping_median();                       
    Serial.print(distancegauche);
    Serial.println("cm gauche");
    myservo.write(90);
  } 

}
    
  else if (sonar.ping_median()> 20){
    stop();    
    forward();
  }

      delay(1000);
}