/*
Les servo sont tel que si leur position 0° se trouve sur le coté "haut", ils peuvent effectuer une rotation vers la position bas par la droite

Quel que soit le mouvement effectué, la position finale du système doit être:
-les deux servo ont la même position, 0 ou 180°
-le servo1 a sa position 180° vers le haut
-le servo2 a sa position 180° vers le bas


Les mouvements possibles sont relatifs au sens du servo1 : l'avant est le sens où pointe la position 180° du servo1, avant ne signifie donc pas forcément le haut

Il faudra donc faire la relation entre le sens d'un déplacement dans l'absolu (haut/bas/droite/gauche) et relatif(avant/arrière/90° sur la droite ou la gauche)
avec un gyroscope.

*/

#include <Servo.h>

Servo servo1;
Servo servo2;
int eaimant1 = 8;     //pins des eaimants et servos
int eaimant2 = 9;
int ser1pos = 5;
int ser2pos = 6;

int pos1 = 0;    // position des servo

int serAv = 1;   //servo le plus en avant relativement à "l'avant" défini par la position 180° du servo1

int delai = 5;

int servoMobile = 0; //servo mobiles et immobiles pour les rotations de 90°
int servoImmo = 0;

void setup() {
  servo1.attach(ser1pos);  // attaches the servo on pin 9 to the servo object
  servo2.attach(ser2pos);
  pinMode(eaimant1, OUTPUT);
  pinMode(eaimant2, OUTPUT);

  servo1.write(0);
  servo2.write(0);
}

void loop() {
  digitalWrite(eaimant1, HIGH);
  digitalWrite(eaimant2, HIGH);

//déplacement en ligne droite:

  if (serAv == 1) digitalWrite(eaimant2, LOW);
  if (serAv == 2) digitalWrite(eaimant1, LOW);
  delay(delai);

  pos1 = servo1.read();           //détermine la position d'un servo, qui est la même que l'autre
  for (pos = pos1; pos <= (180-pos1); pos += 5) //si le servo est à 180° il tourne vers 0°, sinon l'inverse
  {
    servo1.write(pos);            //les servo prennent la variable pos
    servo2.write(pos);
    delay(15);                       // délai standard
  }

  serAv = (3-serAv); //passe de 1 à 2 ou de 2 à 1;
  digitalWrite(eaimant1, HIGH);
  digitalWrite(eaimant2, HIGH);
  delay(delai);
//fin ligne droite

//déplacement arrière:

  if (serAv == 1) digitalWrite(eaimant1, LOW);
  if (serAv == 2) digitalWrite(eaimant2, LOW);
  delay(delai);

  pos1 = servo1.read();
  for (pos = pos1; pos <= (180-pos1); pos += 5)  //si le servo est à 180° il tourne vers 0°, sinon l'inverse
  {
    servo1.write(pos);
    servo2.write(pos);
    delay(15);
  }

  serAv = (3-serAv); //passe de 1 à 2 ou de 2 à 1;
  digitalWrite(eaimant1, HIGH);
  digitalWrite(eaimant2, HIGH);
  delay(delai);
//fin arrière

//rotations 90°

  //du serAv sur la gauche

    if (serAv == 1)
    {
      digitalWrite(eaimant2, LOW);
      delay(delai);

      for (pos = 0; pos <= 90; pos +=5)
      {
        servo1.write(pos);
        delay(15);
      }

      digitalWrite(eaimant2, HIGH);
      delay(delai);
      digitalWrite(eaimant1, LOW);
      delay(delai);

      for (pos = 90; pos >= 0; pos -=5)
     {
       servo1.write(pos);
       delay(15);
      }

      digitalWrite(eaimant1, HIGH);
      delay(delai);
    }
    else if (serAv == 2)
    {
      digitalWrite(eaimant2, LOW);
      delay(delai);

      for (pos = 180; pos >= 90; pos -=5)
      {
        servo2.write(pos);
        delay(15);
      }

      digitalWrite(eaimant2, HIGH);
      delay(delai);
      digitalWrite(eaimant1, LOW);
      delay(delai);

      for (pos = 90; pos <= 180; pos +=5)
      {
       servo2.write(pos);
       delay(15);
      }

      digitalWrite(eaimant1, HIGH);
      delay(delai);
    }
  //fin serAv vers la gauche

  //serAv vers la droite
    if (serAv == 1)
    {
      digitalWrite(eaimant1, LOW);
      delay(delai);

      for (pos = 0; pos <= 90; pos +=5)
      {
        servo1.write(pos);
        delay(15);
      }

      digitalWrite(eaimant1, HIGH);
      delay(delai);
      digitalWrite(eaimant2, LOW);
      delay(delai);

      for (pos = 90; pos >= 0; pos -=5)
      {
       servo1.write(pos);
       delay(15);
      }

      digitalWrite(eaimant2, HIGH);
      delay(delai);
    }
    else if(serAv == 2)
    {
      digitalWrite(eaimant1, LOW);
      delay(delai);

      for (pos = 180; pos >= 90; pos -=5)
      {
        servo2.write(pos);
        delay(15);
      }

      digitalWrite(eaimant1, HIGH);
      delay(delai);
      digitalWrite(eaimant2, LOW);
      delay(delai);

      for (pos = 90; pos <= 180; pos +=5)
      {
       servo2.write(pos);
       delay(15);
      }

      digitalWrite(eaimant1, HIGH);
      delay(delai);
    }
  //fin serAv droite

  //servo arrière vers la gauche
    if (serAv == 1)
    {
      digitalWrite(eaimant2, LOW);
      delay(delai);

      for (pos = 0; pos <= 90; pos +=5)
      {
        servo2.write(pos);
        delay(15);
      }
    }