Controle2ServosRotatifsParJoystick

// ------------------------
// Servo rotatif :
// - valeurs entre 0 et 180
// - 0 : vitesse max dans un sens
// - 90 moteur à l'arrêt
// - 180 vitesse max dans l'autre sens
// ------------------------
// But : utiliser deux servos rotatifs comme moteurs (roues)
// Vitesse et direction contrôlées par joystick
// Principe utilisé :
// la direction du joystick vertical (haut ou bas) indique la vitesse "de base" des moteurs
// la vitesse d'un moteur est modifié pour tourner
// - la direction du joystick horiz (droite ou gauche) donne la direction en étant > 0 ou < 0
// --> < 0 : gauche || > 0 : droite
// - la valeur du joystick indique le pourcentage à retirer de la vitesse du moteur concerné
// --> direction à droite on retire x% de la vitesse du moteur droit pour tourner à droite
// --> direction à gauche on retire x% de la vitesse du moteur gauche pour tourner à gauche
// ------------------------


#include <Servo.h>
//#define DEBUG

Servo servoRotatifDroit;
Servo servoRotatifGauche;

const int brochePotVitesse = A5;
const int brochePotDirection = A0;

int valPotVitesse;
int valPotDirection;
int valVitesseDroit;
int valVitesseGauche;
int valeurMoteurDroit;
int valeurMoteurGauche;

void setup() {

#ifdef DEBUG
  //Serial.begin(9600);
#endif

  servoRotatifDroit.attach(11);
  servoRotatifGauche.attach(6);
  pinMode(brochePotVitesse, INPUT);
  pinMode(brochePotDirection, INPUT);

}

void loop() {

  valPotVitesse = analogRead(brochePotVitesse);
  valVitesseDroit = map(valPotVitesse, 0, 1023, 0, 180);
  valVitesseGauche = map(valPotVitesse, 0, 1023, 180, 0); // vu que les servos sont les même on considère que celui-ci est retourné
  valPotDirection = map(analogRead(brochePotDirection), 0, 1023, -100, 100);

#ifdef DEBUG
  // =DEBUG=====================================================
  /**/Serial.println ("--------------------");
  /**/Serial.println ("Etat potentiomètre");
  /**/Serial.println ("--------------------");
  /**/Serial.print ("vitesse Droite : ");
  /**/Serial.println (valVitesseDroit);
  /**/Serial.print ("vitesse Gauche : ");
  /**/Serial.println (valVitesseGauche);
  /**/Serial.print ("Direction : ");
  /**/Serial.println (valPotDirection);
  /**/Serial.println ("--------------------");
  /**/Serial.println ();
  // =DEBUG=FIN=================================================
#endif

  if (valVitesseDroit > 93 || valVitesseDroit < 87) {  // si la vitesse n'est pas en position centrale (marge de 3)
    // on avance ou on recule
    if (valPotDirection <= 5 && valPotDirection >= -5) {  // si direction est en position centrale (neutre) (marge de 5)

      valeurMoteurDroit = valVitesseDroit;
      valeurMoteurGauche = valVitesseGauche;

      if (valVitesseDroit > 93) {
#ifdef DEBUG
        // =DEBUG=====================================================
        Serial.println ("Tout droit avant");
        // =DEBUG=FIN=================================================
#endif

      } else if (valVitesseDroit < 87) {

#ifdef DEBUG
        // =DEBUG=====================================================
        Serial.println ("Tout droit arriere");
        // =DEBUG=FIN=================================================
#endif

      }

    } else if (valPotDirection < -5) { // tourne gauche
      if (valVitesseGauche < 87) {  // avance (Rappel : inversion du moteur gauche)

#ifdef DEBUG
        // =DEBUG=====================================================
        Serial.println ("Avance Gauche");
        // =DEBUG=FIN=================================================
#endif

        valeurMoteurGauche = valVitesseGauche - (((90 - valVitesseGauche) * valPotDirection) / 100);
        valeurMoteurDroit = valVitesseDroit;

      } else if (valVitesseGauche > 93) {  // recule (Rappel : inversion du moteur gauche)

#ifdef DEBUG
        // =DEBUG=====================================================
        Serial.println ("Recule Gauche");
        // =DEBUG=FIN=================================================
#endif

        valeurMoteurGauche = valVitesseGauche + (((valVitesseGauche - 90) * valPotDirection) / 100);
        valeurMoteurDroit = valVitesseDroit;

      }
    } else if (valPotDirection > 5  ) { // tourne droite
      if (valVitesseDroit > 93) {  // avance

#ifdef DEBUG
        // =DEBUG=====================================================
        Serial.println ("Avance Droite");
        // =DEBUG=FIN=================================================
#endif

        valeurMoteurDroit = valVitesseDroit - (((valVitesseDroit - 90) * valPotDirection) / 100);
        valeurMoteurGauche = valVitesseGauche;

      } else if (valVitesseDroit < 87) {  // recule

#ifdef DEBUG
        // =DEBUG=====================================================
        Serial.println ("Recule Droite");
        // =DEBUG=FIN=================================================
#endif

        valeurMoteurDroit = valVitesseDroit + (((90 - valVitesseDroit) * valPotDirection) / 100);
        valeurMoteurGauche = valVitesseGauche;

      }
    }
  } else if (valVitesseDroit < 93 && valVitesseDroit > 87) {

    // Rajouter la rotation sur plac droite et gauche
    if (valPotDirection < -5) {

      valeurMoteurDroit = map(valPotDirection, 0, 1023, 0, 180);
      valeurMoteurGauche = map(valPotDirection, 0, 1023, 0, 180);

    } else if (valPotDirection > 5) {

      valeurMoteurDroit = map(valPotDirection, 0, 1023, 180, 0);
      valeurMoteurGauche = map(valPotDirection, 0, 1023, 180, 0);

    } else if (valPotDirection > -5 && valPotDirection < 5) {

#ifdef DEBUG
      // =DEBUG=====================================================
      Serial.println ("Stop");
      // =DEBUG=FIN=================================================
#endif

      valeurMoteurDroit = 90;
      valeurMoteurGauche = 90;

    }
  }

#ifdef DEBUG
  // =DEBUG=====================================================
  Serial.println ();
  Serial.println ("--------------------");
  Serial.println ("Etat moteur");
  Serial.println ("--------------------");
  Serial.print ("Valeur Moteur Droit : ");
  Serial.println (valeurMoteurDroit);
  Serial.print ("Valeur Moteur Gauche : ");
  Serial.println (valeurMoteurGauche);
  Serial.println ("--------------------");
  Serial.println ();
  Serial.println ("<<==============================>>");
  Serial.println ();
  // =DEBUG=FIN=================================================
#endif

  servoRotatifDroit.write(valeurMoteurDroit);
  servoRotatifGauche.write(valeurMoteurGauche);

}