/*
--------------------------------------------
Voiture Robotisée avec évitement d'obstacles
--------------------------------------------
Programme pour une voiture robotisée équipée d'un servomoteur et d'un capteur ultrasonique.
La voiture doit pouvoir se déplacer en avant, reculer et tourner à gauche ou à droite pour éviter les obstacles détectés.
En cas de cul-de-sac, la voiture doit reculer et continuer à vérifier les directions disponibles avant d'avancer.
*/

#include <Servo.h>        // Inclusion de la bibliothèque pour contrôler les servomoteurs
#include <Ultrasonic.h>   // Inclusion de la bibliothèque pour le capteur ultrason

// Définir la broche pour le servomoteur
const byte servoPin = 11;

// Définir les broches pour le contrôleur de moteur
const byte moteurGaucheA = 7;         // Moteur gauche Sens avance
const byte moteurGaucheR = 5;         // Moteur gauche Sens recule
const byte moteurGauchePWM = 6;       // Contrôle de vitesse PWM du moteur gauche
const byte moteurDroiteA = 4;         // Moteur droit Sens avance
const byte moteurDroiteR = 2;         // Moteur droit Sens recule
const byte moteurDroitePWM = 3;       // Contrôle de vitesse PWM du moteur droite

// Ajustement pour compenser la différence de vitesse des moteurs
const int vitesseMoteurGauchePWM = 140;  // Vitesse du moteur Gauche (le moins rapide)
const int vitesseMoteurDroitePWM = 100;  // Vitesse du moteur droit (le plus rapide)

// Distance maximale pour les mesures (en cm)
const int distanceMax = 200;

// Seuils de distance pour détection d'obstacle
const byte seuilObstacle = 20;  // Si un obstacle est détecté à moins de 20 cm
const byte seuilCulDeSac = 30;  // Si les distances à gauche et à droite sont inférieures à 30 cm

Servo monServo;                 // Créer un objet Servo pour contrôler le servomoteur
Ultrasonic ultrasonic(13, 12);  // Initialisation du capteur ultrason avec les broches utilisées (Trig, Echo)

void setup() {
  Serial.begin(9600);              // Initialisation de la communication série
  monServo.attach(servoPin);       // Attacher le servomoteur à la broche numérique 11
  monServo.write(90);              // Position initiale du servomoteur (face avant)

  // Initialisation des broches des moteurs
  pinMode(moteurGaucheA, OUTPUT);
  pinMode(moteurGaucheR, OUTPUT);
  pinMode(moteurGauchePWM, OUTPUT);
  pinMode(moteurDroiteA, OUTPUT);
  pinMode(moteurDroiteR, OUTPUT);
  pinMode(moteurDroitePWM, OUTPUT);
}

void loop() {
  int distance = mesurerDistanceMoyenne(); // Mesurer la distance avec le capteur ultrason
  Serial.print("Distance: ");
  Serial.println(distance);

  // Vérifier la distance mesurée pour détecter des obstacles
  if (distance < seuilObstacle) { // Si un obstacle est détecté à moins de 20 cm
    arreterVoiture(); // Arrêter la voiture
    delay(500);

    int distanceGauche = regarderEtMesurer(0);    // Regarder à gauche et mesurer la distance
    int distanceDroite = regarderEtMesurer(180);  // Regarder à droite et mesurer la distance

    monServo.write(90); // Revenir à la position initiale (regarder devant)
    delay(500);

    // Détection du cul-de-sac
    if (distanceGauche < seuilCulDeSac && distanceDroite < seuilCulDeSac) {
      reculer(); // Reculer si dans un cul-de-sac

      // Vérifier à nouveau les distances à gauche et à droite après avoir reculé
      distanceGauche = regarderEtMesurer(0);
      distanceDroite = regarderEtMesurer(180);

      monServo.write(90); // Revenir à la position initiale (regarder devant)
      delay(500);

      // Si toujours pas assez d'espace, continuer à reculer
      while (distanceGauche < seuilCulDeSac && distanceDroite < seuilCulDeSac) {
        reculer(); // Continuer à reculer
        distanceGauche = regarderEtMesurer(0);
        distanceDroite = regarderEtMesurer(180);
        monServo.write(90); // Revenir à la position initiale (regarder devant)
        delay(500);
      }

      // Choisir la direction avec plus d'espace après avoir reculé
      if (distanceGauche > distanceDroite) {
        tournerGauche(); // Tourner à gauche si plus d'espace à gauche
      } else {
        tournerDroite(); // Tourner à droite si plus d'espace à droite
      }

    } else {
      // Tourner vers la direction avec plus d'espace
      if (distanceGauche > distanceDroite) {
        tournerGauche(); // Tourner à gauche si plus d'espace à gauche
      } else {
        tournerDroite(); // Tourner à droite si plus d'espace à droite
      }
    }
  } else {
    avancer(); // Continuer à avancer si aucun obstacle n'est détecté
  }
}

// Fonction pour mesurer la distance avec le capteur ultrason
int mesurerDistanceMoyenne() {
  long totalDistance = 0;
  const int numMesures = 3; // Nombre de mesures pour la moyenne

  // Effectuer plusieurs mesures pour obtenir une moyenne
  for (int i = 0; i < numMesures; i++) {
    int distance = ultrasonic.read(); // Lire la distance mesurée par le capteur ultrason
    if (distance == 0) {
      distance = distanceMax; // Valeur par défaut si aucune mesure valide
    }
    totalDistance += distance; // Ajouter la distance mesurée au total
    delay(10); // Petit délai entre les mesures
  }

  return totalDistance / numMesures; // Retourner la distance moyenne
}

// Fonction pour regarder dans une direction spécifique et mesurer la distance
int regarderEtMesurer(int angle) {
  monServo.write(angle); // Déplacer le servo à l'angle spécifié
  delay(500); // Attendre que le servo se stabilise
  return mesurerDistanceMoyenne(); // Retourner la distance mesurée
}

//*************************************************
//      Fonction qui fait avancer la voiture
//*************************************************
void avancer() {
  digitalWrite(moteurGaucheA, HIGH);  // Mettre la broche du moteur gauche avance à HIGH
  digitalWrite(moteurDroiteA, HIGH);  // Mettre la broche du moteur droit avance à HIGH
  digitalWrite(moteurGaucheR, LOW);   // Mettre la broche du moteur gauche recule à LOW
  digitalWrite(moteurDroiteR, LOW);   // Mettre la broche du moteur droit recule à LOW
  analogWrite(moteurGauchePWM, vitesseMoteurGauchePWM); // Régler la vitesse du moteur gauche
  analogWrite(moteurDroitePWM, vitesseMoteurDroitePWM); // Régler la vitesse du moteur droit
}

//*************************************************
//      Fonction qui stoppe la voiture
//*************************************************
void arreterVoiture() {
  analogWrite(moteurGauchePWM, 100); // Ralentir avant de s'arrêter (moteur gauche)
  analogWrite(moteurDroitePWM, 100); // Ralentir avant de s'arrêter (moteur droit)
  delay(200);
  digitalWrite(moteurGaucheA, LOW);  // Arrêter le moteur gauche avance
  digitalWrite(moteurGaucheR, LOW);  // Arrêter le moteur gauche recule
  digitalWrite(moteurDroiteA, LOW);  // Arrêter le moteur droit avance
  digitalWrite(moteurDroiteR, LOW);  // Arrêter le moteur droit recule
}

//*************************************************
//      Fonction qui fait tourner la voiture à gauche
//*************************************************
void tournerGauche() {
  digitalWrite(moteurGaucheA, LOW);  // Arrêter le moteur gauche avance
  digitalWrite(moteurDroiteA, HIGH); // Faire avancer le moteur droit
  digitalWrite(moteurGaucheR, LOW);  // Faire reculer le moteur gauche
  digitalWrite(moteurDroiteR, LOW);  // Faire reculer le moteur droit
  delay(600);       // Ajuster le délai pour un virage plus précis
  arreterVoiture(); // Arrêter la voiture après le virage
}

//*************************************************
//      Fonction qui fait tourner la voiture à droite
//*************************************************
void tournerDroite() {
  digitalWrite(moteurGaucheA, HIGH); // Faire avancer le moteur gauche
  digitalWrite(moteurDroiteA, LOW);  // Arrêter le moteur droit avance
  digitalWrite(moteurGaucheR, LOW);  // Faire reculer le moteur gauche
  digitalWrite(moteurDroiteR, LOW);  // Faire reculer le moteur droit
  delay(600);       // Ajuster le délai pour un virage plus précis
  arreterVoiture(); // Arrêter la voiture après le virage
}

//*************************************************
//      Fonction qui fait reculer la voiture
//*************************************************
void reculer() {
  digitalWrite(moteurGaucheA, LOW);   // Faire avancer le moteur gauche
  digitalWrite(moteurDroiteA, LOW);   // Faire avancer le moteur droit
  digitalWrite(moteurGaucheR, HIGH);  // Arrêter le moteur gauche recule
  digitalWrite(moteurDroiteR, HIGH);  // Arrêter le moteur droit recule
  analogWrite(moteurGauchePWM, vitesseMoteurGauchePWM); // Régler la vitesse du moteur gauche
  analogWrite(moteurDroitePWM, vitesseMoteurDroitePWM); // Régler la vitesse du moteur droit
  delay(1000);      // Reculer pendant 1 seconde (ajuster si nécessaire)
  arreterVoiture(); // Arrêter la voiture après le recul
}