#include <Grove_I2C_Motor_Driver.h>
// default I2C address is 0x0f
#define I2C_ADDRESS 0x0f

const int capteurGauche = 2;//Capteur Gauche
const int capteurCentre = 5;//Capteur Avant
const int capteurDroit = 6;//Capteur Droit
const int capteurMilieu = 3;//Capteur Milieu

bool etatCapteurGauche;
bool etatCapteurCentre;
bool etatCapteurDroit;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  pinMode(capteurGauche, INPUT);
  pinMode(capteurCentre, INPUT);
  pinMode(capteurDroit, INPUT);
  Motor.begin(I2C_ADDRESS);
}

void loop() {
  // put your main code here, to run repeatedly:
  etatCapteurGauche = digitalRead(capteurGauche);
  etatCapteurCentre = digitalRead(capteurCentre);
  etatCapteurDroit = digitalRead(capteurDroit);

  if(etatCapteurCentre) //Si le capteur du centre détecte du noir
  {
    if(!etatCapteurGauche && etatCapteurDroit){
      Motor.speed(MOTOR1, -50);
      // Set speed of MOTOR2, Anticlockwise
      Motor.speed(MOTOR2, 50);
    }
    else if(etatCapteurGauche && !etatCapteurDroit)
    {
       Motor.speed(MOTOR1, 50);
       // Set speed of MOTOR2, Anticlockwise
       Motor.speed(MOTOR2, -50);
    }
    else {
       Motor.speed(MOTOR1, 100);
       // Set speed of MOTOR2, Anticlockwise
       Motor.speed(MOTOR2, 100);
  }
  }
  else if(!etatCapteurCentre) //Si le capteur du centre détecte du blanc
  {
    if(!etatCapteurGauche && etatCapteurDroit)
    {
       Motor.speed(MOTOR1, -50);
       // Set speed of MOTOR2, Anticlockwise
       Motor.speed(MOTOR2, 50);
    }
    else if(etatCapteurGauche && !etatCapteurDroit)
    {
       Motor.speed(MOTOR1, 50);
       // Set speed of MOTOR2, Anticlockwise
       Motor.speed(MOTOR2, -50);
    }
    else {
      Motor.speed(MOTOR1, 85);
      Motor.speed(MOTOR2, -85);
      delay(1150);
    }
}
  else {
      Motor.stop(MOTOR1);
      Motor.stop(MOTOR2);
    }
}