//Moteur A connecté entre les pins A01 et A02 du Motor Driver TB6612FNG
//Moteur B connecté entre les pins B01 et B02 du Motor Driver TB6612FNG
 
int STBY = 10; // standby Pin (si HIGH alors on peut faire tourner les moteurs, si LOW, les moteurs s'arrêtent)
 
//Motor A
int PWMA = 5; //Pin qui contrôle la vitesse du moteur A (nombre entre 0 et 255 donc on doit le mettre sur un port PWM)
// AIN1 et AIN2 sont les pins de sens de rotation du moteur A
// Le moteur A tourne dans le sens horaire si le pin AIN1 est LOW et le pin AIN2 est HIGH 
// Inversement, si AIN1 est HIGH et AIN2 est LOW, il tourne dans le sens antihoraire
// (si les deux sont LOW ou les deux sont HIGH, il ne tourne pas)
int AIN1 = 9; //Sens de rotation
int AIN2 = 8; //Sens
 
//Motor B
int PWMB = 3; //Pin qui contrôle la vitesse du moteur B
// Pins de sens de rotation du moteur B
int BIN1 = 11; //Sens
int BIN2 = 12; //Sens
 
void setup() {
  // On définit tous les ports comme sorties pour piloter les moteurs
  pinMode(STBY, OUTPUT);
 
  // Commandes Moteur A
  pinMode(PWMA, OUTPUT);
  pinMode(AIN1, OUTPUT);
  pinMode(AIN2, OUTPUT);
 
  // Commandes Moteur B
  pinMode(PWMB, OUTPUT);
  pinMode(BIN1, OUTPUT);
  pinMode(BIN2, OUTPUT);
}
 
void loop() {
  move(1, 200, 1); //motor 1, full speed, left
  move(2, 200, 1); //motor 2, full speed, left
 
  delay(5000); //on les laisse tourner 5 seconde
  stop(); // on stoppe les moteurs
  delay(250); //hold for 250ms until move again
 
  move(1, 128, 0); //motor 1, half speed, right
  move(2, 128, 0); //motor 2, half speed, right
 
  delay(1000);
  stop();
  delay(250);
}
 
void move(int motor, int speed, int direction) {
  // Function move(): Move specific motor at speed and direction
  //
  // Inputs:
  // motor: 0 for B and 1 for A
  // speed: number between 0 and 255: 0 is off, and 255 is full speed
  // direction: 0 clockwise, 1 counter-clockwise
 
  // DEMARRAGE
  digitalWrite(STBY, HIGH); //On met ce Pin sur HIGH avant de démarrer les moteurs
 
  // SENS DE ROTATION
  // Sens Horaire
  // Le moteur A tourne dans le sens horaire si le pin AIN1 est LOW et le pin AIN2 est HIGH (si les deux sont LOW ou les deux sont HIGH, il ne tourne pas)
  boolean inPin1 = LOW;
  boolean inPin2 = HIGH;
  // Sens Anti-Horaire
  if (direction == 1) { // Si on change la direction dans le sens antihoraire
    inPin1 = HIGH;
    inPin2 = LOW;
  }
 
  // ENVOI DES INSTRUCTIONS DE SENS DE ROTATION ET DE VITESSE AUX MOTEURS
  if (motor == 1) { // MOTEUR A
    digitalWrite(AIN1, inPin1);
    digitalWrite(AIN2, inPin2);
    analogWrite(PWMA, speed);
  } else { // if motor == 0 (MOTEUR B)
    digitalWrite(BIN1, inPin1);
    digitalWrite(BIN2, inPin2);
    analogWrite(PWMB, speed);
  }
}
 
void stop() {
  //Function stop(): Appeler cette fonction dans le programme pour stopper les moteurs
  digitalWrite(STBY, LOW);
}