#include <Servo.h> // Inclusion de la bibliothèque Servo pour contrôler les servomoteurs

// Création de 4 objets Servo pour contrôler les différents servomoteurs
Servo servoMilieu, servoGauche, servoDroite, servoPince;

/**
 * Déplace un servo d'une position initiale à une position finale dans un temps imparti.
 * @param servo Servo à déplacer
 * @param impulsionDepart Largeur d'impulsion de départ en microsecondes
 * @param impulsionFin Largeur d'impulsion finale en microsecondes
 * @param duree Durée du mouvement en millisecondes
 */
void deplacerServo(Servo &servo, int impulsionDepart, int impulsionFin, unsigned long duree) {
  int nombreDePas = 100;  // Nombre de pas pour le mouvement afin d'assurer un déplacement fluide
  unsigned long delaiEntrePas = duree / nombreDePas;  // Délai entre chaque pas en millisecondes

  // Boucle pour déplacer le servo progressivement de la position initiale à la position finale
  for (int i = 0; i <= nombreDePas; i++) {
    int position = map(i, 0, nombreDePas, impulsionDepart, impulsionFin);  // Calcul de la position intermédiaire du servo
    servo.writeMicroseconds(position);  // Déplacement du servo à la position intermédiaire
    delay(delaiEntrePas);  // Attente avant le prochain déplacement
  }
}

void setup() {
  Serial.begin(9600);  // Initialisation de la communication série pour le débogage

  // Attachement des servos aux broches correspondantes
  servoMilieu.attach(11);  // Servo pour le mouvement de rotation sur la broche 11
  servoGauche.attach(10);  // Servo pour le bras gauche sur la broche 10
  servoDroite.attach(9);   // Servo pour le bras droit sur la broche 9
  servoPince.attach(6);    // Servo pour la pince sur la broche 6

  // Position de départ pour chaque servo (en microsecondes)
  servoMilieu.writeMicroseconds(1500);  // Position neutre du servo de rotation
  servoGauche.writeMicroseconds(1500);  // Position neutre du servo du bras gauche
  servoDroite.writeMicroseconds(1500);  // Position neutre du servo du bras droit
  servoPince.writeMicroseconds(2000);   // Position fermée de la pince
  delay(5000);  // Attente de 5 secondes avant de commencer les mouvements

  // Définir la durée pour chaque mouvement en millisecondes
  unsigned long dureeDeMouvement = 1500;  // 1,5 seconde pour chaque mouvement

  // Séquence de mouvements pour les bras et la pince

  // Monte le bras gauche
  deplacerServo(servoGauche, 1500, 2000, dureeDeMouvement);

  // Tourne à gauche
  deplacerServo(servoMilieu, 1500, 2000, dureeDeMouvement);

  // Ouvre la pince
  deplacerServo(servoPince, 2000, 1000, dureeDeMouvement);

  // Baisse le bras gauche
  deplacerServo(servoGauche, 2000, 1000, dureeDeMouvement);

  // Descend le bras droit
  deplacerServo(servoDroite, 1500, 2000, dureeDeMouvement);

  // Ferme la pince
  deplacerServo(servoPince, 1000, 2000, dureeDeMouvement);

  // Monte le bras droit
  deplacerServo(servoDroite, 2000, 1500, dureeDeMouvement);

  // Monte le bras gauche
  deplacerServo(servoGauche, 1000, 2000, dureeDeMouvement);

  // Tourne à gauche
  deplacerServo(servoMilieu, 2000, 1000, dureeDeMouvement);

  // Baisse le bras gauche
  deplacerServo(servoGauche, 2000, 1000, dureeDeMouvement);

  // Descend le bras droit
  deplacerServo(servoDroite, 1500, 2000, dureeDeMouvement);

  // Ouvre la pince
  deplacerServo(servoPince, 2000, 1000, dureeDeMouvement);

  // Monte le bras droit
  deplacerServo(servoDroite, 2000, 1500, dureeDeMouvement);

  // Monte le bras gauche
  deplacerServo(servoGauche, 1000, 2000, dureeDeMouvement);

  // Tourne à droite pour revenir à la position initiale
  deplacerServo(servoMilieu, 1000, 1500, dureeDeMouvement);

  // Ferme la pince
  deplacerServo(servoPince, 1000, 2000, dureeDeMouvement);

  // Baisse le bras gauche pour revenir à la position initiale
  deplacerServo(servoGauche, 2000, 1500, dureeDeMouvement);
}

void loop() {
  // La fonction loop est vide car toutes les actions se passent dans setup()
}