CSMA CAR v3.2.13

/*              CSMA   :  CHIALI  Sidi  Mohammed  El  Amine

 
                   *  *        * * * * * * * * * * * * * * *       * * * * * * * *      
                *            *          *      *      *      *
              *               *         *      *      *       *
             *                  *       *      *      *       *
             *                     *    *      *      * * * * *    * * * *        
              *                      *  *      *      *       *
                *                    *  *      *      *       *
                   *  *  *  *  *   *    *      *      *       *    * * 
*/

/*
Note :
- The attached program is not optimal and the comments are written in French !!
*/

 #include <PS3BT.h>
 #include <usbhub.h>
 #include <Servo.h>
 // Satisfaire l'IDE, qui doit voir l'instruction include dans l'introduction
 #ifdef dobogusinclude
 #include <spi4teensy3.h>
 #endif
 #include <SPI.h>
 #include <NewPing.h>

 #define SONAR_NUM     3             // Nombre de capteurs
 #define MAX_DISTANCE  50            // Distance maximale (en cm) à ping.
 #define PING_INTERVAL 10            // Millisecondes entre les pings du capteur (29 ms correspond à peu près au minimum pour éviter l'écho de capteurs croisés).
 unsigned long pingTimer[SONAR_NUM]; // Indique le moment où le prochain ping devrait se produire pour chaque capteur.
 unsigned int cm[SONAR_NUM];         // Où les distances de ping sont stockées.
 uint8_t currentSensor = 0;          // Garde la trace du capteur actif.

 // Tableau d'objets capteurs            US DROIT                        US MILIEU                       US GAUCHE
 NewPing sonar[SONAR_NUM] = { NewPing(28, 29, MAX_DISTANCE) , NewPing(30, 31, MAX_DISTANCE) , NewPing(32, 33, MAX_DISTANCE) } ; //  NewPing( La broche TRIG , la broche Echo , la distance maximale par ping de chaque capteur)
 
 USB Usb;
 
 //USBHub HUB1 (& USB); Certains dongles ont un hub à l'intérieur

 BTD Btd(&Usb); // You have to create the Bluetooth Dongle instance like so
 /* Vous pouvez créer l'instance de la classe de deux façons */
 //PS3BT PS3(&Btd); // This will just create the instance
 PS3BT PS3(&Btd, 0x00, 0x15, 0x83, 0x3D, 0x0A, 0x57); // This will also store the bluetooth address - this can be obtained from the dongle when running the sketch

 int K = 0 ; 
 bool state = 0 ;

 
 bool ETAT = 0 ;
 
 bool POWER = 0 ;
 bool Triangle = 0 ,Cercle = 0 , Croix = 0 , Carre = 0 ;
 bool r1 = 0 , l1 = 0 ;
 bool Haut = 0 , Bas = 0 , Droite = 0 , Gauche = 0 ;

 int pinDirA = 22;             //  MOTEUR A
 int pinBrkA = 23;             //  MOTEUR A
 int pinPwmA = 2;              //  MOTEUR A
 
 int pinDirB = 24;             //  MOTEUR B
 int pinBrkB = 25;             //  MOTEUR B
 int pinPwmB = 3;              //  MOTEUR B

 int JoygaucheX,JoygaucheY,JoydroitX,JoydroitY;
 int Avant,Arriere;
 float TANGAGE , ROULIS ;

 
 int Vitesse ;             // Mode AUTO
 int Cmd_Vitesse = 175 ;   // Mode AUTO
 int WARNING = 10 ;        // Distance maximale qui stop les moteurs
 
 int pos1 = 100  ;
 int pos2 = 180 ;
 int posG = 0   ;      //   position servo GAUCHE
 int posD = 0   ;      //   position servo DROIT

 unsigned long previous_time_servo = 0 ;
 unsigned long delay_servo = 5000 ;


 Servo Direction   ;
 Servo servodroit  ;
 Servo servogauche ;

 
 int ANGLE;

 const int buzzer = 36 ;
 


 const int Led1 = 18 ;
 const int Led2 = 19 ;
 const int Led3 = 20 ; 
 const int Led4 = 21 ;


 
 unsigned long previous_time_blink = 0 ;    // Temps précédent         
 unsigned long delay_blink = 300 ;          // Intervalle entre les clignotements des LEDS  300 ms 


 void setup() {

 pingTimer[0] = millis() + 75;                         // Le premier ping commence à 75 ms, donne le temps à l’Arduino de refroidir avant de commencer.
 for (uint8_t i = 1; i < SONAR_NUM; i++)               // Définissez l'heure de démarrage pour chaque capteur.
      pingTimer[i] = pingTimer[i - 1] + PING_INTERVAL;


 pinMode(buzzer, OUTPUT);
 
 pinMode(Led1,OUTPUT);
 pinMode(Led2,OUTPUT);
 pinMode(Led3,OUTPUT);
 pinMode(Led4,OUTPUT);


 
  
 Direction.attach(45)  ;
 servodroit.attach(6)  ;
 servogauche.attach(5) ;
 
 servodroit.write(100);      // Position initiale du servo Droit
 servogauche.write(80);      // Position initiale du servo Gauche




 pinMode(pinDirA, OUTPUT);
 pinMode(pinBrkA, OUTPUT);
 pinMode(pinPwmA, OUTPUT);
 pinMode(pinDirB, OUTPUT);
 pinMode(pinBrkB, OUTPUT);
 pinMode(pinPwmB, OUTPUT);  

  Serial.begin(115200);
  #if !defined(__MIPSEL__)
  while (!Serial); // Attendez que le port série se connecte-utilisé sur Leonardo, teensy et d'autres conseils avec construit en USB CDC connexion série
  #endif
  if (Usb.Init() == -1) {
    Serial.print(F("\r\nOSC did not start"));
  while (1);  //Arrêter
  }
  Serial.print(F("\r\nPS3 Bluetooth Library Started"));
 }


 void loop() {
 
                 
Usb.Task();
  
if (PS3.PS3Connected || PS3.PS3NavigationConnected) {


if ( PS3.getAnalogButton(L2) > 100 && PS3.getButtonClick(R1)){             // Si je touche les boutons Analog (L2) et (R1) en même temps, la variable "POWER" s'incrémente         
    POWER = ! POWER ;
    STOP (); 
    K = 0 ;
    REINITIALISER_TOUS_LES_MODES () ;                       
    while ( K < 2 ) {
           ANIMATION_AU_DEMARRAGE ( HIGH ) ;
           delay (125);
           ANIMATION_AU_DEMARRAGE ( LOW ) ;
           delay (125);
           K++; 
    }
    
    digitalWrite(buzzer, LOW );
    ETEINDRE_LES_LEDS ();                                   }

if (PS3.getButtonClick(L1))                                 {             //  Si je Touche le bouton ( L1 ) , La variable "l1" s'incrémente           
    l1 = ! l1 ;                                             }

if (PS3.getButtonClick(R1))                                 {             //  Si je Touche le bouton ( R1 ) , La variable "r1" s'incrémente           
    r1 = ! r1 ;                                             }
     
if (PS3.getButtonClick(UP))                                 {             //  Si je Touche le bouton ( HAUT ) , La variable "Haut" s'incrémente           
    Haut = ! Haut ;                                         }

if (PS3.getButtonClick(RIGHT))                              {             //  Si je Touche le bouton ( DROITE ) , La variable "Droite" s'incrémente           
    Droite = ! Droite ;                                     }

if (PS3.getButtonClick(DOWN))                               {             //  Si je Touche le bouton ( BAS ) , La variable "Bas" s'incrémente           
    Bas = ! Bas ;                                           }

if (PS3.getButtonClick(LEFT))                               {             //  Si je Touche le bouton ( GAUCHE ) , La variable "Gauche" s'incrémente           
    Gauche = ! Gauche ;                                     }
    
if (PS3.getButtonClick(SQUARE))                             {             //  Si je Touche le bouton ( CARRE ) , La variable "Carre" s'incrémente           
    Carre = ! Carre ;                                       }
    
if (PS3.getButtonClick(CROSS))                              {             //  Si je Touche le bouton ( CROIX) , La variable "Croix" s'incrémente           
    Croix = ! Croix ;
    Cercle   = 0 ;
    Triangle = 0 ;                                          }
    
if (PS3.getButtonClick(CIRCLE))                             {             //  Si je Touche le bouton ( CERCLE) , La variable "Cercle" s'incrémente           
    Cercle = ! Cercle ;      
    Croix    = 0 ;
    Triangle = 0 ;                                          }
    
if (PS3.getButtonClick(TRIANGLE))                           {             //  Si je Touche le bouton ( TRIANGLE) , La variable "Triangle" s'incrémente           
    Triangle = ! Triangle ;                     
    Croix  = 0 ;
    Cercle = 0 ;                                            }                 



//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%//
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%//


if ( POWER == 0 ) {                          //      !!!   STOP   !!!
   
    STOP ();                                 // Arrêter les Moteurs
    ETEINDRE_LES_LEDS ();                    // Eteindre les LEDS
    Direction.write(96);                     // Position initiale de la Direction
    servodroit.write(100);                   // Position initiale du Servo Droit
    servogauche.write(80);                   // Position initiale du Servo Gauche
    Serial.println("   ");
}

if ( POWER == 1 ) {                          //      !!!   START   !!!

  if ( Croix == 1 ) {                        //      !!!   Mode   " MOUVEMENT "   Activé   !!!
   
   TIMER ();
   TANGAGE = PS3.getAngle(Pitch) ;         // AVANCER / RECULER
   ROULIS  = PS3.getAngle(Roll)  ;         // TOURNER A DROITE / TOURNER A GAUCHE
   
   if ( PS3.getAngle(Pitch) > 190 ) {      // Balancer la manette vers l'avant pour Avancer
      CLIGNOTEMENT_EN_AVANT ();
      Vitesse = map(TANGAGE, 190 , 240 , 50 , 255 ) ;
      AVANCEZ ( Vitesse ) ;
                                    }
      
   else if (PS3.getAngle(Pitch) < 170 ) {  // Balancer la manette vers l'arrière pour RECULER
      CLIGNOTEMENT_EN_ARRIERE ();
      Vitesse = map(TANGAGE, 170 , 120 , 50, 255 ) ;
      RECULEZ ( Vitesse ) ;
                                        } 

   else{                                   // Maintenir la manette équilibrée pour STOPPER les moteurs
      STOP ();
      ETEINDRE_LES_LEDS ();}

   if (PS3.getAngle(Roll) < 170){         // Balancer la manette vers la gauche pour tourner à GAUCHE
      CLIGNOTEMENT_A_GAUCHE ();
      ANGLE = map( ROULIS , 120 , 170 , 70 , 95 );   
      Direction.write(ANGLE);
                                }                     
   else if (PS3.getAngle(Roll) > 190){    // Balancer la manette vers la droite pour tourner à DROITE                        
      CLIGNOTEMENT_A_DROITE ();
      ANGLE = map( ROULIS , 190 ,240 , 97 , 122 );   
      Direction.write(ANGLE);
                                     }        
   else {
      ETEINDRE_LES_LEDS ();
      Direction.write(96);                // Maintenir la manette équilibrée pour Centrer la direction
        }
        
   Serial.print(F("\r\nTANGAGE: "));
   Serial.print(PS3.getAngle(Pitch));
   Serial.print(F("\tROULIS: "));
   Serial.print(PS3.getAngle(Roll));   
   }

//-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------//
//-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------//

  if ( Triangle == 0 ) {            //      !!!   Mode   " AUTO "   Désactivé   !!!
   
    servodroit.write(100);                   // Position initiale du Servo Droit
    servogauche.write(80);                   // Position initiale du Servo Gauche
    if( Croix  == 0 && Cercle == 0) {
    STOP ();
    }
  }
  
  if ( Triangle == 1 ) {            //      !!!   Mode   " AUTO "   Activé   !!!

    for (uint8_t i = 0; i < SONAR_NUM; i++) {                                       // Boucle à travers tous les capteurs.
         if (millis() >= pingTimer[i]) {                                            // Est-ce que c'est l'heure de ce capteur?
             pingTimer[i] += PING_INTERVAL * SONAR_NUM;                             // Défini la prochaine fois que ce capteur sera envoyé.
             if (i == 0 && currentSensor == SONAR_NUM - 1) oneSensorCycle();        // Le cycle de ping du capteur est terminé, faites quelque chose avec les résultats.
                 sonar[currentSensor].timer_stop();                                 // Assurez-vous que la minuterie précédente est annulée avant de commencer un nouveau ping (assurance).
                 currentSensor = i;                                                 // Capteur en cours d'accès.
                 cm[currentSensor] = 0;                                             // Définissez la distance zéro au cas où il n’y aurait pas d’écho de ping pour ce capteur.
                 sonar[currentSensor].ping_timer(echoCheck);                        // Faites le ping (le traitement continue, interruption appellera echoCheck pour rechercher un écho).
                                       }
                                            }
                     }
                     
//-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------//
//-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------//

  
  if ( Cercle == 1 ) {              //      !!!   Mode   " Manuel "   Activé   !!!    

   TIMER ();
   STOP ();
   JoygaucheX =   PS3.getAnalogHat(LeftHatX)    ;
   JoygaucheY =   PS3.getAnalogHat(LeftHatY)    ;
   JoydroitX  =   PS3.getAnalogHat(RightHatX)   ;
   JoydroitY  =   PS3.getAnalogHat(RightHatY)   ;
   Avant      =   PS3.getAnalogButton(R2)       ;
   Arriere    =   PS3.getAnalogButton(L2)       ;
   
   if (JoygaucheX < 117){
      CLIGNOTEMENT_A_GAUCHE ();
      ANGLE = map(JoygaucheX,0,117,70,96);   
      Direction.write(ANGLE);
                        }
   else if (JoygaucheX > 137){                           
      CLIGNOTEMENT_A_DROITE ();
      ANGLE = map(JoygaucheX,137,255,96,122); 
      Direction.write(ANGLE);
                             }
   else {
      ETEINDRE_LES_LEDS ();
      Direction.write(96); 
        }

   if ( PS3.getAnalogHat(RightHatX) > 137 ){                             //           Direction char (TOURNEZ A DROITE)
      FEUX_DE_DETRESSE ();
      Vitesse = map(JoydroitX, 137,255,50,255) ;
      TOURNEZ_A_DROITE ( Vitesse ) ;
                                           }
   else if (PS3.getAnalogHat(RightHatX) < 117) {                         //           Direction char (TOURNEZ A GAUCHE)
      FEUX_DE_DETRESSE ();
      Vitesse =  map(JoydroitX, 117,0,50,255) ;
      TOURNEZ_A_GAUCHE ( Vitesse ) ;
                                               } 
                               
   if ( PS3.getAnalogButton(R2)) {                                       //           AVANCEZ
      Vitesse = map(Avant, 0, 255, 50, 255 ) ;
      AVANCEZ ( Vitesse ) ;      }
      
   else if (PS3.getAnalogButton(L2)) {
      Vitesse = map(Arriere, 0, 255, 50, 180 ) ;                         //           RECULEZ
      RECULEZ ( Vitesse ) ;          } 
     


           
   Serial.print   (" Angle : ");
   Serial.print   (ANGLE);
   Serial.print   ("°");
   Serial.print   ("   Vitesse AVANT/ARRIERE : ");
   Serial.print   (Avant);
   Serial.print   ("/");
   Serial.println   (Arriere);
   } 

//-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------//
//-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------//

}    
  if (PS3.getButtonClick(PS)) {            //     Éteindre la manette
      Serial.print(F("\r\nPS"));
      PS3.disconnect();                              
                              }
                              


//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%//
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%//

} 
}
 
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
 
  void echoCheck() {                        // Si la commande ping a été reçue, définissez la distance du capteur sur Array.
  if (sonar[currentSensor].check_timer())
      cm[currentSensor] = sonar[currentSensor].ping_result / US_ROUNDTRIP_CM;
  }

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
  
  void oneSensorCycle() {                   //Le cycle de ping du capteur est terminé, faites quelque chose avec les résultats.

  for (uint8_t i = 0; i < SONAR_NUM; i++) {
    Serial.print(i)    ;
    Serial.print("=")  ;
    Serial.print(cm[i]);
    Serial.print("cm ");                  }
  Serial.println();
  // stocker les résultats dans des variables   

  int DROIT  = cm[0];
  int MILIEU = cm[1];
  int GAUCHE = cm[2];

if( DROIT == 0 && MILIEU == 0 && GAUCHE == 0 ){  // Si aucun des Capteurs ne détectent la distance souhaité
  if ( micros() - previous_time_servo > delay_servo ) { 
          STOP ();    // Fonction qui permet de STOPER les moteurs
          if( pos1 <= 180 ){                                                                          
             posD = 180 - pos1 ;
             posG = pos1 ;
             servodroit .write( posD );
             servogauche.write( posG );
             pos1 = pos1 + 10 ;
                            }
          if( pos1 > 180 ){
             posD = 180 - pos2 ;
             posG = pos2 ;
             servodroit .write( posD );
             servogauche.write( posG ); 
             pos2 = pos2 - 10 ;
             if( pos2 == -10 ){ pos2 = 180 ; pos1 = 100 ;}}
      previous_time_servo = micros()  ;                   }
}




if( DROIT > 0 && MILIEU > 0 && GAUCHE > 0 ){  // Si tous les Capteurs détectent la distance souhaité
  
   if( DROIT < MILIEU &&  DROIT < GAUCHE  ){      // si la Distance du Capteur droit est la plus inférieur 
      Vitesse = Cmd_Vitesse ;
      TOURNEZ_A_DROITE ( Vitesse ) ;                      // Fonction qui permet de Tourner a droite avec une Vitesse réglable
   }

   if( MILIEU < DROIT &&  MILIEU < GAUCHE ){      // si la Distance du Capteur milieu est la plus inférieur
      if ( MILIEU < WARNING ){
          STOP ();                                // Fonction qui permet de STOPER les moteurs
                             }
      else {
      Vitesse = Cmd_Vitesse ;
      AVANCEZ          ( Vitesse ) ;   }                   // Fonction qui permet d'Avancer tout droit avec une Vitesse réglable 
   }

   if( GAUCHE < DROIT &&  GAUCHE < MILIEU ){      // si la Distance du Capteur gauche est la plus inférieur
      Vitesse = Cmd_Vitesse ;
      TOURNEZ_A_GAUCHE ( Vitesse ) ;                      // Fonction qui permet de Tourner a gauche avec une Vitesse réglable
   }}




if( DROIT > 0 && MILIEU == 0 && GAUCHE > 0 ){  // Si le Capteur gauche et le Capteur droit détectent la distance souhaité

   if( DROIT < GAUCHE ){    // si la Distance du Capteur droit est la plus inférieur
      Vitesse = Cmd_Vitesse ;
      TOURNEZ_A_DROITE ( Vitesse ) ;                      // Fonction qui permet de Tourner a droite avec une Vitesse réglable 
   }

   if( GAUCHE < DROIT ){    // si la Distance du Capteur gauche est la plus inférieur
      Vitesse = Cmd_Vitesse ;
      TOURNEZ_A_GAUCHE ( Vitesse ) ;                      // Fonction qui permet de Tourner a gauche avec une Vitesse réglable 
   }}




if( DROIT == 0 && MILIEU > 0 && GAUCHE > 0 ){  // Si le Capteur gauche et le Capteur milieu détectent la distance souhaité

   if( MILIEU < GAUCHE ){    // si la Distance du Capteur milieu est la plus inférieur
      if ( MILIEU < WARNING ){
          STOP ();                                // Fonction qui permet de STOPER les moteurs
                             }
      else {
      Vitesse = Cmd_Vitesse ;
      AVANCEZ          ( Vitesse ) ;  }                   // Fonction qui permet d'Avancer tout droit avec une Vitesse réglable  
   }

   if( GAUCHE < MILIEU ){    // si la Distance du Capteur gauche est la plus inférieur
      Vitesse = Cmd_Vitesse ;
      TOURNEZ_A_GAUCHE ( Vitesse ) ;                      // Fonction qui permet de Tourner a gauche avec une Vitesse réglable 
   }}




if( DROIT > 0 && MILIEU > 0 && GAUCHE == 0 ){  // Si le Capteur droit et le Capteur milieu détectent la distance souhaité

   if( DROIT < MILIEU ){    // si la Distance du Capteur droit est la plus inférieur
      Vitesse = Cmd_Vitesse ;
      TOURNEZ_A_DROITE ( Vitesse ) ;                      // Fonction qui permet de Tourner a droite avec une Vitesse réglable  
   }

   if( MILIEU < DROIT ){    // si la Distance du Capteur milieu est la plus inférieur
      if ( MILIEU < WARNING ){
          STOP ();                                // Fonction qui permet de STOPER les moteurs
                             }
      else {
      Vitesse = Cmd_Vitesse ;
      AVANCEZ          ( Vitesse ) ;   }                  // Fonction qui permet d'Avancer tout droit avec une Vitesse réglable  
   }}




if( DROIT > 0 && MILIEU == 0 && GAUCHE == 0 ){  // Si seulement le Capteur droit qui détecte la distance souhaité
   Vitesse = Cmd_Vitesse ;
   TOURNEZ_A_DROITE ( Vitesse ) ;                      // Fonction qui permet de Tourner a droite avec une Vitesse réglable 
}




if( DROIT == 0 && MILIEU > 0 && GAUCHE == 0 ){  // Si seulement le Capteur milieu qui détecte la distance souhaité
   if ( MILIEU < WARNING ){
          STOP ();                                // Fonction qui permet de STOPER les moteurs
                             }
      else {
   Vitesse = Cmd_Vitesse ;
   AVANCEZ          ( Vitesse ) ;  }                   // Fonction qui permet d'Avancer tout droit avec une Vitesse réglable 
}




if( DROIT == 0 && MILIEU == 0 && GAUCHE > 0 ){  // Si seulement le Capteur gauche qui détecte la distance souhaité
   Vitesse = Cmd_Vitesse ;
   TOURNEZ_A_GAUCHE ( Vitesse ) ;                      // Fonction qui permet de Tourner a gauche avec une Vitesse réglable
}

}

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//

void AVANCEZ ( int Vitesse ) {
     digitalWrite(pinDirA,HIGH)      ;
     digitalWrite(pinBrkA,LOW )      ;
     digitalWrite(pinDirB,HIGH)      ;
     digitalWrite(pinBrkB,LOW )      ;
     analogWrite (pinPwmA, Vitesse ) ;
     analogWrite (pinPwmB, Vitesse ) ;   
}

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//

void RECULEZ ( int Vitesse ) {
     digitalWrite(pinDirA,LOW )      ;
     digitalWrite(pinBrkA,HIGH)      ;
     digitalWrite(pinDirB,LOW )      ;
     digitalWrite(pinBrkB,HIGH)      ;
     analogWrite (pinPwmA, Vitesse ) ;
     analogWrite (pinPwmB, Vitesse ) ;   
}

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//

void TOURNEZ_A_DROITE ( int Vitesse ) {
     digitalWrite(pinDirA,HIGH)      ;
     digitalWrite(pinBrkA,LOW )      ;
     digitalWrite(pinDirB,LOW )      ;
     digitalWrite(pinBrkB,HIGH)      ;                                                             
     analogWrite (pinPwmA, Vitesse ) ;
     analogWrite (pinPwmB, Vitesse ) ;
}

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//

void TOURNEZ_A_GAUCHE ( int Vitesse ) {
     digitalWrite(pinDirA,LOW )      ;
     digitalWrite(pinBrkA,HIGH)      ;
     digitalWrite(pinDirB,HIGH)      ;
     digitalWrite(pinBrkB,LOW )      ;                                                             
     analogWrite (pinPwmA, Vitesse ) ;
     analogWrite (pinPwmB, Vitesse ) ;      
}

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//

void STOP (){
     digitalWrite(pinDirA,LOW ) ;
     digitalWrite(pinBrkA,LOW)  ;
     digitalWrite(pinDirB,LOW)  ;
     digitalWrite(pinBrkB,LOW ) ;  
}

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
 
void TIMER (){                              // TIMER : Fonction qui permet de faire une incrémentation sans délai
     if ( millis() - previous_time_blink > delay_blink ) {
         ETAT = !ETAT ;
         previous_time_blink = millis();                 }   
}
  
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//

void FEUX_DE_DETRESSE (){                   // FEUX DE DETRESSE
     digitalWrite(Led1 , ETAT );  
     digitalWrite(Led2 , ETAT ); 
     digitalWrite(Led3 , ETAT );
     digitalWrite(Led4 , ETAT );
}

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//

void CLIGNOTEMENT_A_DROITE (){              // CLIGNOTEMENT A DROITE
     digitalWrite(Led1 , LOW  );  
     digitalWrite(Led2 , ETAT ); 
     digitalWrite(Led3 , LOW  );
     digitalWrite(Led4 , ETAT );
}

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//

void CLIGNOTEMENT_A_GAUCHE (){              // CLIGNOTEMENT A GAUCHE
     digitalWrite(Led1 , ETAT );  
     digitalWrite(Led2 , LOW  ); 
     digitalWrite(Led3 , ETAT );
     digitalWrite(Led4 , LOW  );
}

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//

void CLIGNOTEMENT_EN_AVANT (){              // CLIGNOTEMENT EN AVANT
     digitalWrite(Led1 , ETAT );  
     digitalWrite(Led2 , ETAT ); 
     digitalWrite(Led3 , LOW  );
     digitalWrite(Led4 , LOW  );
}

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//

void CLIGNOTEMENT_EN_ARRIERE (){            // CLIGNOTEMENT EN ARRIERE
     digitalWrite(Led1 , LOW  );  
     digitalWrite(Led2 , LOW  ); 
     digitalWrite(Led3 , ETAT );
     digitalWrite(Led4 , ETAT );
}

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//

void ETEINDRE_LES_LEDS (){                  // ETEINDRE LES LEDS
     digitalWrite(Led1 , LOW  );  
     digitalWrite(Led2 , LOW  ); 
     digitalWrite(Led3 , LOW  );
     digitalWrite(Led4 , LOW  );
}

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//

void ANIMATION_AU_DEMARRAGE ( bool state ) {            // BIP + CLIGNOTEMENTS AU DÉMARRAGE
     
     digitalWrite(buzzer, state );
     digitalWrite(Led1  , state );  
     digitalWrite(Led2  , state ); 
     digitalWrite(Led3  , state );
     digitalWrite(Led4  , state );    
}

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
  
void REINITIALISER_TOUS_LES_MODES (){       // REINITIALISER TOUS LES MODES
     Croix    = 0 ;
     Cercle   = 0 ;
     Triangle = 0 ;   
     PS3.setRumbleOn(RumbleLow); 
}

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//