/////////////////////////////////////////////////////////////////////////////////////
//                             Created by : Théo GAUTIER                           //
//                                 FabAcademy : 2021                               //
//                           AgriLab, week 12 (output week)                        //
//  Modulation of the speed of the motors according to the distance from the user  //
///////////////////////////////////////////////////////////////////////////////////// 

         
//moteurs

int M1 = 5;  //Moteur 1
int M2 = 14;  //Moteur 2
int M3 = 15;  //Moteur 3
int M4 = 4;  //Moteur 4

// Ultrason

int TRIG = 8; //borne trigger de l'ultrason
int ECHO = 9; //borne echo de l'ultrason
long lecture_echo;
long cm;



void setup()
{
 Serial.begin(115200); //nombre de baud pour le moniteur série

pinMode (M1, OUTPUT); //déclare que M1 est une sortie
pinMode (M2, OUTPUT); //déclare que M2 est une sortie
pinMode (M3, OUTPUT); //déclare que M3 est une sortie
pinMode (M4, OUTPUT); //déclare que M4 est une sortie
pinMode (TRIG, OUTPUT);//Emetteur ultrasons
digitalWrite(TRIG, LOW);
pinMode (ECHO, INPUT); //Récepteur ultrasons

}

void go()
{

analogWrite (M1, 240); //Fait tourner le moteur à 95% de sa vitesse
analogWrite (M2, 240); //Fait tourner le moteur à 95% de sa vitesse
analogWrite (M3, 240); //Fait tourner le moteur à 95% de sa vitesse
analogWrite (M4, 240); //Fait tourner le moteur à 95% de sa vitesse

}

void stop()
{

analogWrite (M1, 0); //Fait tourner le moteur à 0% de sa vitesse
analogWrite (M2, 0); //Fait tourner le moteur à 0% de sa vitesse
analogWrite (M3, 0); //Fait tourner le moteur à 0% de sa vitesse
analogWrite (M4, 0); //Fait tourner le moteur à 0% de sa vitesse

}

void slow()
{
  
analogWrite (M1, 100); //Fait tourner le moteur à 40% de sa vitesse
analogWrite (M2, 100); //Fait tourner le moteur à 40% de sa vitesse
analogWrite (M3, 100); //Fait tourner le moteur à 40% de sa vitesse
analogWrite (M4, 100); //Fait tourner le moteur à 40% de sa vitesse

}

void loop() 

{

digitalWrite(TRIG, HIGH);  
  delayMicroseconds(10); 
  digitalWrite(TRIG, LOW);
  lecture_echo = pulseIn(ECHO, HIGH);
  cm = lecture_echo /58;
  Serial.print("Distance en cm :");
  Serial.println(cm);


if (cm <= 30)
{
  Serial.println("Arret d'urgence");
   stop();
}

else if (cm <= 50) 
{ // 50 >= cm > 30
  Serial.println("Tranquillement");
   slow();
}

else // cm > 50
{ 
  Serial.println("Go !");
   go();

}
}