//exemple pris de : https://gist.github.com/WoLpH/0094b278a54f8f472c76b7eb0d0a8f89

#include <Servo.h>

//Constantes:
#define PWM_PIN  11
#define REDLED_PIN 6
#define GREENLED_PIN  5
#define TRIG_PIN 3
#define ECHO_PIN 2

//Variables
int pos =0;
//variable pour la led inverse
int inv_pos =180;
int start_stop_motor = 1;
float distance = 0.0;
float duration = 0.0;

//Classes
Servo servo1;

//Functions
void blinking_led();
void turn_motor(int stop_start_motor);
void distance_sonar();

void setup() {
  Serial.begin(9600);

//initialisation des Pins
  pinMode(ECHO_PIN, INPUT_PULLUP);
  pinMode(TRIG_PIN, OUTPUT);

//initalisation du moteur
  servo1.attach(PWM_PIN);
  servo1.write(pos);
}

void loop() {

  turn_motor(start_stop_motor);

}

//fonction qui permet de flasher les LED
void blinking_led(){
    for(int i=0; i < 3; i++ ){  
      digitalWrite(GREENLED_PIN, HIGH);
      digitalWrite(REDLED_PIN, HIGH);
      delay(1000);
      digitalWrite(GREENLED_PIN, LOW);
      digitalWrite(REDLED_PIN, LOW);
      delay(1000);
  }
}

//Fonction qui permet de tourner les moteurs et controler les LED
void turn_motor(int stop_start_motor){

//tourne le moteur dans le sens horaire selon un certain degré max
    for (pos = 0; pos <= 180; pos++) {
    if(pos == 0) delay(1000);

    distance_sonar();

    if(stop_start_motor ==0 ){
      //arrêt du moteur et led
      analogWrite(GREENLED_PIN, 0);  
      analogWrite(REDLED_PIN, 0);
      servo1.write(0);

    }
    else {
      servo1.write(pos);
      //calcul du PWM 180 degré * 255/degré max
      analogWrite(GREENLED_PIN, pos*255/180);  
      analogWrite(REDLED_PIN, inv_pos*255/180);           
      delay(100);
      inv_pos--;                     
    }
  }
  //deuxieme boucle, moteur tourne dans le sense anti-horaire
    for (pos = 180; pos >= 0; pos--) {
    if(pos == 180) delay(1000);

    distance_sonar();
    
    if(stop_start_motor ==0 ){
      analogWrite(GREENLED_PIN, 0);  
      analogWrite(REDLED_PIN, 0);
      servo1.write(0);

    }
    else {
      servo1.write(pos);
    //calcul du PWM 180 degré * 255/degré max
      analogWrite(GREENLED_PIN, pos*255/180); 
      analogWrite(REDLED_PIN, inv_pos*255/180);               
      delay(100);
      inv_pos++;
      }                   
    }
}

//Fonction qui envoie le ping et traite l'echo
void distance_sonar(){

//Envoyer le ping surt le trigger
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

//récupérer le temps et calculer la distance
  duration = pulseIn(ECHO_PIN, HIGH);
  distance = duration * 0.034 / 2;

  Serial.print("Distance : "); 
  Serial.println(distance);

//arret des moteurs si un objet est détecté à moins de 30 cm
  if(distance <= 30){
    blinking_led();
    start_stop_motor = 0;
  }

  else {
    start_stop_motor = 1;
  }



}