#include <SoftwareSerial.h>
#include <Servo.h>

// Création de 4 objets Servo pour contrôler les différents servomoteurs
Servo milieu, gauche, droite, pince;

// Configuration de la communication série via Bluetooth HC-06
SoftwareSerial BT(9, 8);  // RX côté Arduino sur 9, TX côté Arduino sur 8 (connecté à TX/RX du module HC-06)

// Positions initiales des servos (en degrés)
int posSerMilieu = 90;   // Position initiale du servo de rotation
int posServoGauche = 90; // Position initiale du servo du bras gauche
int posServoDroite = 90; // Position initiale du servo du bras droit
int posServoPince = 170; // Position initiale de la pince (fermée)

// Variable pour stocker la commande reçue via Bluetooth
char valSerie;

void setup()
{
    // Initialisation des communications série
    Serial.begin(9600); // Démarrage de la communication série pour le moniteur série
    BT.begin(9600);     // Démarrage de la communication série pour le module Bluetooth HC-06

    // Attachement des servos aux broches correspondantes
    milieu.attach(11); // On attache l'objet servo milieu sur la broche 11
    gauche.attach(13); // On attache l'objet servo gauche sur la broche 13
    droite.attach(12); // On attache l'objet servo droite sur la broche 12
    pince.attach(10);  // On attache l'objet servo pince sur la broche 10

    // Déplacement des servos vers leurs positions de départ
    milieu.write(posSerMilieu);   // Servo milieu en position de départ
    gauche.write(posServoGauche); // Servo gauche en position de départ
    droite.write(posServoDroite); // Servo droite en position de départ
    pince.write(posServoPince);   // Pince en position de départ (fermée)
    delay(2000); // Pause pour s'assurer que tous les servos atteignent leurs positions initiales
}

/******************************************************/
/*   Fonction qui fait pivoter le bras à gauche       */
/******************************************************/
void brasAGauche()
{
    if (posSerMilieu > 10) // Vérifie si la position n'atteint pas la limite minimale
    {
        posSerMilieu = posSerMilieu - 2; // Diminue la position du servo par pas de 2 degrés
        milieu.write(posSerMilieu);      // Met à jour la position du servo
        delay(50); // Petite pause pour que le mouvement soit fluide et visible
    }
}

/******************************************************/
/*   Fonction qui fait pivoter le bras à droite       */
/******************************************************/
void brasADroite()
{
    if (posSerMilieu < 170) // Vérifie si la position n'atteint pas la limite maximale
    {
        posSerMilieu = posSerMilieu + 2; // Augmente la position du servo par pas de 2 degrés
        milieu.write(posSerMilieu);      // Met à jour la position du servo
        delay(50); // Petite pause pour que le mouvement soit fluide et visible
    }
}

/******************************************************/
/*   Fonction qui ouvre la pince                      */
/******************************************************/
void pinceOuverte()
{
    if (posServoPince > 70) // Vérifie si la position n'atteint pas la limite d'ouverture maximale
    {
        posServoPince = posServoPince - 2; // Réduit la position de la pince pour l'ouvrir
        pince.write(posServoPince);        // Met à jour la position de la pince
        delay(30); // Petite pause pour que le mouvement soit fluide et visible
    }
}

/******************************************************/
/*   Fonction qui ferme la pince                      */
/******************************************************/
void pinceFermee()
{
    if (posServoPince < 180) // Vérifie si la position n'atteint pas la limite de fermeture maximale
    {
        posServoPince = posServoPince + 2; // Augmente la position de la pince pour la fermer
        pince.write(posServoPince);        // Met à jour la position de la pince
        delay(30); // Petite pause pour que le mouvement soit fluide et visible
    }
}

/******************************************************/
/*   Fonction qui avance le bras                      */
/******************************************************/
void brasAvance()
{
    if (posServoDroite < 160) // Vérifie si la position n'atteint pas la limite maximale d'avance
    {
        posServoDroite = posServoDroite + 2; // Augmente la position du bras droit
        droite.write(posServoDroite);        // Met à jour la position du servo droit
        delay(30); // Petite pause pour que le mouvement soit fluide et visible
    }
}

/******************************************************/
/*   Fonction qui recule le bras                      */
/******************************************************/
void brasRecule()
{
    if (posServoDroite > 80) // Vérifie si la position n'atteint pas la limite minimale de recul
    {
        posServoDroite = posServoDroite - 2; // Diminue la position du bras droit
        droite.write(posServoDroite);        // Met à jour la position du servo droit
        delay(30); // Petite pause pour que le mouvement soit fluide et visible
    }
}

/******************************************************/
/*   Fonction qui monte le bras                       */
/******************************************************/
void brasMonte()
{
    if (posServoGauche < 150) // Vérifie si la position n'atteint pas la limite maximale de montée
    {
        posServoGauche = posServoGauche + 2; // Augmente la position du bras gauche
        gauche.write(posServoGauche);        // Met à jour la position du servo gauche
        delay(30); // Petite pause pour que le mouvement soit fluide et visible
    }
}

/******************************************************/
/*   Fonction qui descend le bras                     */
/******************************************************/
void brasDescend()
{
    if (posServoGauche > 0) // Vérifie si la position n'atteint pas la limite minimale de descente
    {
        posServoGauche = posServoGauche - 2; // Diminue la position du bras gauche
        gauche.write(posServoGauche);        // Met à jour la position du servo gauche
        delay(30); // Petite pause pour que le mouvement soit fluide et visible
    }
}

void loop()
{
    // Lecture de la commande envoyée via Bluetooth
    if (BT.available()) // Si des données sont disponibles sur le port série Bluetooth
    {
        valSerie = BT.read(); // Lire la commande reçue
        Serial.print("Valeur lue : "); // Affiche la valeur reçue sur le moniteur série
        Serial.println(valSerie);      // Affiche la commande reçue pour débogage
    }

    // Exécution de la commande reçue
    if (valSerie != 'S') // Ignore si la commande est 'S' (Stop)
    {
        if (valSerie == 'G')
            brasAGauche(); // Appelle la fonction pour pivoter le bras à gauche
        if (valSerie == 'D')
            brasADroite(); // Appelle la fonction pour pivoter le bras à droite
        if (valSerie == 'O')
            pinceOuverte(); // Appelle la fonction pour ouvrir la pince
        if (valSerie == 'F')
            pinceFermee(); // Appelle la fonction pour fermer la pince
        if (valSerie == 'M')
            brasMonte(); // Appelle la fonction pour monter le bras
        if (valSerie == 'E')
            brasDescend(); // Appelle la fonction pour descendre le bras
        if (valSerie == 'A')
            brasAvance(); // Appelle la fonction pour avancer le bras
        if (valSerie == 'R')
            brasRecule(); // Appelle la fonction pour reculer le bras
    }
}