/* 
Controle de deux servo 360 avec quatre boutons.
La déclaration en tant que pullup des boutons ne rend pas indispensable résistance sur le bouton car elle en active une intégrée dans l'arduino.
CW = Clockwise = Sens horaire
CCW = Counter clockwise = Sens Anti-horaire
CC-BY-NC-SA Pouvoir Faire
 */
 
#include <Servo.h>

Servo myservoG;  // Créé le servo Gauche
Servo myservoD;  // Crée le servo Droit
Servo myservoA;  // Crée le servo Droit

int servoPinG = 9;// Branchement du servo gauche sur la pin 9
int servoPinD = 10;// Branchement du servo droit sur la pin 10
int servoPinA = 11;

int AvancePin = 2; //  Bouton pour avancer sur la pin 2
int ReculePin = 3; // Bouton pour reculer sur la pin 3
int GauchePin = 4; // Bouton pour aller à gauche sur la pin 4
int DroitePin = 5; // Bouton pour aller à droite sur la pin 5
int ArmePin = 6;
        
int CCW = 120; // Valeur du servo pour le Sens anti horaire
int CW = 60; // Valeur du servo pour le Sens horaire
int Sstop = 93; // Valeur du servo pour le Stop, il se peut que le servo ne s'arrete pas complètement, cela est du à une impressision de fabrication
int pos = 0 ;


void setup() {
  Serial.begin(9600);
  
  pinMode(AvancePin,INPUT_PULLUP);// declare la pin du bouton avance comme un bouton, ne rend pas indispensable résistance sur le bouton
  pinMode(ReculePin,INPUT_PULLUP);//  declare la pin du bouton recule comme un bouton
  pinMode(GauchePin,INPUT_PULLUP);//  declare la pin du bouton gauche comme un bouton
  pinMode(DroitePin,INPUT_PULLUP);//  declare la pin du bouton droite comme un bouton
  pinMode(ArmePin,INPUT_PULLUP);//  declare la pin du bouton droite comme un bouton

  myservoG.write(Sstop);// Initialise avec les moteur arrétés
  
}



void loop() {
for (pos = 0; pos <= 180; pos += 1) { // goes from 0 degrees to 180 degrees
    // in steps of 1 degree
    myservoA.write(pos);              // tell servo to go to position in variable 'pos'
    delay(15);                       // waits 15ms for the servo to reach the position
  }
  for (pos = 180; pos >= 0; pos -= 1) { // goes from 180 degrees to 0 degrees
    myservoA.write(pos);              // tell servo to go to position in variable 'pos'
    delay(15);       
bool    Avance = digitalRead(AvancePin);// Lit le statut du bouton avance et le met dans la variable Avance
bool    Recule = digitalRead(ReculePin);// Lit le statut du bouton recule et le met dans la variable Recule
bool    Gauche = digitalRead(GauchePin);// lit le statut du bouton gauche et le met dans la variable Gauche
bool    Droite = digitalRead(DroitePin); // Lit le statut du bouton droit et le met dans la variable Droite
    

if (Avance == LOW){ // Si le bouton avancer est appuyé, envoi la fonction avancer
      Favancer();
  }
else if (Recule == LOW){ // Si le bouton reculer est appuyé, envoi la fonction reculer
   Freculer();
  }
else if (Gauche == LOW){ // Si le bouton gauche est appuyé, envoi la fonction Aller a gauche
    Fgauche();
  }
else if (Droite == LOW){ // Si le bouton Droite est appuyé, envoi la fonction Aller a droite
     Fdroit();
}
else 
  {
    Fstop();  //Si rien n'est appuyé, envoi la fonction stop
  }
  
}

void Favancer(){ //Fonction Avancer
 myservoG.attach(servoPinG);  // Attache le servo en pin 9 sur l'objet myservoG
  myservoD.attach(servoPinD);  // Attache le servo en pin 10 sur l'objet myservoD
  
  
  myservoG.write(CW); // Fait tourner le moteur dans le sens horaire
  myservoD.write(CCW);// Fait tourner le moteur dans le sens Anti-horaire
}

void Freculer(){  //Fonction Reculer
  myservoG.attach(servoPinG);  // Attache le servo en pin 9 sur l'objet myservoG
  myservoD.attach(servoPinD);  // Attache le servo en pin 10 sur l'objet myservoD
  
  myservoG.write(CCW); // Fait tourner le moteur dans le sens Anti-horaire
  myservoD.write(CW); // Fait tourner le moteur dans le sens horaire

}

void Fdroit(){ //fonction Aller à droite
  myservoG.attach(servoPinG);  // Attache le servo en pin 9 sur l'objet myservoG
  myservoD.attach(servoPinD);  // Attache le servo en pin 10 sur l'objet myservoD
     
  myservoG.write(CCW);// Fait tourner le moteur dans le sens Anti-horaire
  myservoD.write(CCW);// Fait tourner le moteur dans le sens Anti-horaire

}

void Fgauche(){  //fonction Aller à gauche
  myservoG.attach(servoPinG);  // Attache le servo en pin 9 sur l'objet myservoG
  myservoD.attach(servoPinD);  // Attache le servo en pin 10 sur l'objet myservoD
     
  myservoG.write(CW); // Fait tourner le moteur dans le sens horaire
  myservoD.write(CW); // Fait tourner le moteur dans le sens horaire
    
}

void Fstop(){    //Fonction Stop
    myservoG.detach(); // ces deux commandes ont été rajouté car parfois les roues du bot continuaient à trouver même en ayant envoyé le signal STOP
    myservoD.detach(); // ces deux commandes ont été rajouté car parfois les roues du bot continuaient à trouver même en ayant envoyé le signal STOP

}