/*
--------------------------------------------
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.
*/

#include <Servo.h>        // Librairie Servomoteur
#include "Ultrasonic.h"   // Librairie Ultrason

// Déclarations Servomoteur
Servo monServo;                    // Créer un objet Servo pour contrôler le servomoteur
const int servoPin = 11;            // Broche 11 pour le servomoteur
const byte distanceObstacle = 20;   // Distance en centimètres pour détecter un obstacle
int distance;                       // Variable pour stocker la distance mesurée
int distanceAvant;                  // Distance mesurée devant
int distanceDroite;                 // Distance mesurée à droite
int distanceGauche;                 // Distance mesurée à gauche

// Déclarations Ultrason
Ultrasonic ultrasonic(13, 12);      // Initialisation du capteur ultrason avec les broches utilisées (Trig, Echo)

// Déclarations Moteurs
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 droit

const int vitesseMoteurGauchePWM = 140;  // Vitesse du moteur gauche (le moins rapide)
const int vitesseMoteurDroitePWM = 100;  // Vitesse du moteur droit (le plus rapide)

void setup() {
  Serial.begin(9600);              // Initialisation de la communication série pour le débogage
  monServo.attach(servoPin);       // Attacher le servomoteur à la broche numérique PIN 11
  monServo.write(90);              // Position initiale du servo (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);
  
  // Configurer la vitesse initiale des moteurs
  analogWrite(moteurGauchePWM, vitesseMoteurGauchePWM);
  analogWrite(moteurDroitePWM, vitesseMoteurDroitePWM);
}

//*************************************************
//      Fonction qui fait avancer la voiture
//*************************************************
void avancerVoiture() {
  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
}

//*************************************************
//      Fonction qui stoppe la voiture
//*************************************************
void arreterVoiture() {
  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);  // Arrêter le moteur gauche recule
  digitalWrite(moteurDroiteR, LOW);  // Arrêter le moteur droit recule
}

//*************************************************
//      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);  // Arrêter le moteur gauche recule
  digitalWrite(moteurDroiteR, LOW);  // Arrêter le moteur droit recule
}

//*************************************************
//      Fonction pour mesurer la distance à l'avant
//*************************************************
void mesurerDistanceAvant() {
  monServo.write(90);                // Positionner le servo face avant
  distanceAvant = ultrasonic.read(); // Lire la distance avec le capteur ultrason
}

//*************************************************
//      Fonction pour mesurer la distance à gauche
//*************************************************
void mesurerDistanceGauche() {
  monServo.write(150);               // Positionner le servo à gauche
  distanceGauche = ultrasonic.read();// Lire la distance avec le capteur ultrason
}

//*************************************************
//      Fonction pour mesurer la distance à droite
//*************************************************
void mesurerDistanceDroite() {
  monServo.write(30);                // Positionner le servo à droite
  distanceDroite = ultrasonic.read();// Lire la distance avec le capteur ultrason
}

void loop() {
  mesurerDistanceAvant();            // Mesurer la distance devant
  delay(50);                         // Attendre un court instant

  if (distanceAvant < distanceObstacle) { // Si la distance avant est inférieure à la distance d'obstacle
    arreterVoiture();                  // Arrêter la voiture
    delay(300);                        // Attendre un court instant
    mesurerDistanceGauche();           // Mesurer la distance à gauche
    delay(200);                        // Attendre un court instant
    mesurerDistanceDroite();           // Mesurer la distance à droite
    delay(200);                        // Attendre un court instant

    // Choisir la direction avec plus d'espace
    if (distanceGauche > distanceDroite) {
      tournerGauche();                 // Tourner à gauche
      delay(500);                      // Attendre un court instant
    } else {
      tournerDroite();                 // Tourner à droite
      delay(500);                      // Attendre un court instant
    }
  } else {
    avancerVoiture();                  // Avancer la voiture si aucun obstacle n'est détecté
  }
}