//Définition des ports pour controler les moteurs
//Moteur gauche :
#define IN1a 27
#define IN2a 14
#define IN3a 12
#define IN4a 13
//Moteur droit :
#define IN1b 5
#define IN2b 18
#define IN3b 19
#define IN4b 21

//Temps d'attente entre deux impulsions :
int delayTime = 3;

//Code éxécuté au démarrage (paramétrage) :
void setup() {
  //On indique que les ports de controle des moteurs sont des sorties (output)
  pinMode(IN1a, OUTPUT);
  pinMode(IN2a, OUTPUT);
  pinMode(IN3a, OUTPUT);
  pinMode(IN4a, OUTPUT);

  pinMode(IN1b, OUTPUT);
  pinMode(IN2b, OUTPUT);
  pinMode(IN3b, OUTPUT);
  pinMode(IN4b, OUTPUT);


  Serial.begin(115200);  //Démarrage d'une communication Série avec l'ordinateur s'il est connecté

  delay(2000);

  // DEBUT DU CODE
  // Fonctions disponibles : forward(<distance en cm>) | backward(<distance en cm>) | left(<rotation en degré>) | right(<rotation en degré>)

  

  // FIN DU CODE
}

//Code éxécuté en boucle jusqu'à l'extinction du robot
void loop() {
}



//Fonction pour convertir une longueur en cm vers un nombre de pas moteur.
int convertLengthToSteps(float length) {
  float result = length * 512 / (4 * 3.1415);
  return int(result);
}

//fonction pour convertir une rotation en degrés vers un nombre de pas moteur
int convertRotToSteps(int rotation) {
  int result = convertLengthToSteps(rotation * 3.1415 / 180 * 7.8);
  return result;
}


void forward(int length) {
  for (int i = 0; i < convertLengthToSteps(length); i++) {
    digitalWrite(IN4a, HIGH);
    digitalWrite(IN3a, LOW);
    digitalWrite(IN2a, LOW);
    digitalWrite(IN1a, LOW);
    digitalWrite(IN4b, LOW);
    digitalWrite(IN3b, LOW);
    digitalWrite(IN2b, LOW);
    digitalWrite(IN1b, HIGH);
    delay(delayTime);
    digitalWrite(IN4a, LOW);
    digitalWrite(IN3a, HIGH);
    digitalWrite(IN2a, LOW);
    digitalWrite(IN1a, LOW);
    digitalWrite(IN4b, LOW);
    digitalWrite(IN3b, LOW);
    digitalWrite(IN2b, HIGH);
    digitalWrite(IN1b, LOW);
    delay(delayTime);
    digitalWrite(IN4a, LOW);
    digitalWrite(IN3a, LOW);
    digitalWrite(IN2a, HIGH);
    digitalWrite(IN1a, LOW);
    digitalWrite(IN4b, LOW);
    digitalWrite(IN3b, HIGH);
    digitalWrite(IN2b, LOW);
    digitalWrite(IN1b, LOW);
    delay(delayTime);
    digitalWrite(IN4a, LOW);
    digitalWrite(IN3a, LOW);
    digitalWrite(IN2a, LOW);
    digitalWrite(IN1a, HIGH);
    digitalWrite(IN4b, HIGH);
    digitalWrite(IN3b, LOW);
    digitalWrite(IN2b, LOW);
    digitalWrite(IN1b, LOW);
    delay(delayTime);
  }
}

void backward(int length) {
  for (int i = 0; i < convertLengthToSteps(length); i++) {
    digitalWrite(IN4a, LOW);
    digitalWrite(IN3a, LOW);
    digitalWrite(IN2a, LOW);
    digitalWrite(IN1a, HIGH);
    digitalWrite(IN4b, HIGH);
    digitalWrite(IN3b, LOW);
    digitalWrite(IN2b, LOW);
    digitalWrite(IN1b, LOW);
    delay(delayTime);
    digitalWrite(IN4a, LOW);
    digitalWrite(IN3a, LOW);
    digitalWrite(IN2a, HIGH);
    digitalWrite(IN1a, LOW);
    digitalWrite(IN4b, LOW);
    digitalWrite(IN3b, HIGH);
    digitalWrite(IN2b, LOW);
    digitalWrite(IN1b, LOW);
    delay(delayTime);
    digitalWrite(IN4a, LOW);
    digitalWrite(IN3a, HIGH);
    digitalWrite(IN2a, LOW);
    digitalWrite(IN1a, LOW);
    digitalWrite(IN4b, LOW);
    digitalWrite(IN3b, LOW);
    digitalWrite(IN2b, HIGH);
    digitalWrite(IN1b, LOW);
    delay(delayTime);
    digitalWrite(IN4a, HIGH);
    digitalWrite(IN3a, LOW);
    digitalWrite(IN2a, LOW);
    digitalWrite(IN1a, LOW);
    digitalWrite(IN4b, LOW);
    digitalWrite(IN3b, LOW);
    digitalWrite(IN2b, LOW);
    digitalWrite(IN1b, HIGH);
    delay(delayTime);
  }
}

void right(int deg) {
  for (int i = 0; i < convertRotToSteps(deg); i++) {
    digitalWrite(IN4a, HIGH);
    digitalWrite(IN3a, LOW);
    digitalWrite(IN2a, LOW);
    digitalWrite(IN1a, LOW);
    digitalWrite(IN4b, HIGH);
    digitalWrite(IN3b, LOW);
    digitalWrite(IN2b, LOW);
    digitalWrite(IN1b, LOW);
    delay(delayTime);
    digitalWrite(IN4a, LOW);
    digitalWrite(IN3a, HIGH);
    digitalWrite(IN2a, LOW);
    digitalWrite(IN1a, LOW);
    digitalWrite(IN4b, LOW);
    digitalWrite(IN3b, HIGH);
    digitalWrite(IN2b, LOW);
    digitalWrite(IN1b, LOW);
    delay(delayTime);
    digitalWrite(IN4a, LOW);
    digitalWrite(IN3a, LOW);
    digitalWrite(IN2a, HIGH);
    digitalWrite(IN1a, LOW);
    digitalWrite(IN4b, LOW);
    digitalWrite(IN3b, LOW);
    digitalWrite(IN2b, HIGH);
    digitalWrite(IN1b, LOW);
    delay(delayTime);
    digitalWrite(IN4a, LOW);
    digitalWrite(IN3a, LOW);
    digitalWrite(IN2a, LOW);
    digitalWrite(IN1a, HIGH);
    digitalWrite(IN4b, LOW);
    digitalWrite(IN3b, LOW);
    digitalWrite(IN2b, LOW);
    digitalWrite(IN1b, HIGH);
    delay(delayTime);
  }
}

void left(int deg) {
  for (int i = 0; i < convertRotToSteps(deg); i++) {
    digitalWrite(IN4a, LOW);
    digitalWrite(IN3a, LOW);
    digitalWrite(IN2a, LOW);
    digitalWrite(IN1a, HIGH);
    digitalWrite(IN4b, LOW);
    digitalWrite(IN3b, LOW);
    digitalWrite(IN2b, LOW);
    digitalWrite(IN1b, HIGH);
    delay(delayTime);
    digitalWrite(IN4a, LOW);
    digitalWrite(IN3a, LOW);
    digitalWrite(IN2a, HIGH);
    digitalWrite(IN1a, LOW);
    digitalWrite(IN4b, LOW);
    digitalWrite(IN3b, LOW);
    digitalWrite(IN2b, HIGH);
    digitalWrite(IN1b, LOW);
    delay(delayTime);
    digitalWrite(IN4a, LOW);
    digitalWrite(IN3a, HIGH);
    digitalWrite(IN2a, LOW);
    digitalWrite(IN1a, LOW);
    digitalWrite(IN4b, LOW);
    digitalWrite(IN3b, HIGH);
    digitalWrite(IN2b, LOW);
    digitalWrite(IN1b, LOW);
    delay(delayTime);
    digitalWrite(IN4a, HIGH);
    digitalWrite(IN3a, LOW);
    digitalWrite(IN2a, LOW);
    digitalWrite(IN1a, LOW);
    digitalWrite(IN4b, HIGH);
    digitalWrite(IN3b, LOW);
    digitalWrite(IN2b, LOW);
    digitalWrite(IN1b, LOW);
    delay(delayTime);
  }
}


void stopMotors(void) {
  digitalWrite(IN4a, LOW);
  digitalWrite(IN3a, LOW);
  digitalWrite(IN2a, LOW);
  digitalWrite(IN1a, LOW);

  digitalWrite(IN4b, LOW);
  digitalWrite(IN3b, LOW);
  digitalWrite(IN2b, LOW);
  digitalWrite(IN1b, LOW);
}
