/*
  Bras robotisé à 4 DOF (degrés de liberté) avec 4 servos SG90,
  Piloté par 2 joysticks (mode manuel) et capable d'une marche automatique (séquence programmée)
  Mode Automatique et Manuel sélectionné par l'appui d'un bouton poussoir (avec leds mode auto manu)
 */

 // bibliothèque de gestion servo moteurs
 #include <Servo.h>

 // bibliothèque de rebond, facilite grandement le travail avec les boutons et les interrupteurs
#include <Bounce2.h>

// Broches des Servomoteurs SG90 (à adapter)
#define SERVO_BASE_PIN 9
#define SERVO_EPAULE_PIN 10
#define SERVO_COUDE_PIN 11
#define SERVO_PINCE_PIN 12

// Broches du Joystick (Module double joystick, à adapter)
// Joystick 1 - Contrôle X/Y
#define JOY1_X_PIN A0 
#define JOY1_Y_PIN A1 
#define JOY1_SW_PIN 7 // Bouton du Joystick 1 (pour une fonction spéciale, par exemple, pas utilisé pour l'instant)


// Joystick 2 - Contrôle X/Y
#define JOY2_X_PIN A2
#define JOY2_Y_PIN A3 
#define JOY2_SW_PIN 8 // Bouton du Joystick 2 (pour une fonction spéciale, par exemple, pas utilisé pour l'instant)

// interrupteur changement de mode
#define buttonPin 2 
// configurez l'objet bouton en utilisant la bibliothèque de rebond
Bounce button = Bounce();
// la variable pour conserver votre état lors du basculement entre les sous-routines
bool state;

#define LEDPIN_AUTO 3  // Led verte pour mode automatique
#define LEDPIN_MANU 4  // Led bleu pour mode manuel

// Variables pour les servomoteurs
Servo servoBase;
Servo servoEpaule;
Servo servoCoude;
Servo servoPince;

// Variables de position (angles en degrés)
int posBase = 90;
int posEpaule = 90;
int posCoude = 90;
int posPince = 90;


void manuelControl() {
  // Lire les valeurs des joysticks (entre 0 et 1023)
  int valJoy1_X = analogRead(JOY1_X_PIN);
  int valJoy1_Y = analogRead(JOY1_Y_PIN);
  int valJoy2_X = analogRead(JOY2_X_PIN);
  int valJoy2_Y = analogRead(JOY2_Y_PIN);

  // --- Contrôle par Hystérésis (Déplacement par pas) ---

  // Joystick 1 - X (Base)
  if (valJoy1_X < 400) {
    posBase = max(0, posBase - 1); // Diminuer l'angle (vers 0°)
  } else if (valJoy1_X > 600) {
    posBase = min(180, posBase + 1); // Augmenter l'angle (vers 180°)
  }

  // Joystick 1 - Y (Épaule/Coude)
  if (valJoy1_Y < 400) {
    posEpaule = max(0, posEpaule - 1); 
  } else if (valJoy1_Y > 600) {
    posEpaule = min(180, posEpaule + 1); 
  }

  // Joystick 2 - X (Coude/Pince)
  if (valJoy2_X < 400) {
    posCoude = max(0, posCoude - 1); 
  } else if (valJoy2_X > 600) {
    posCoude = min(180, posCoude + 1); 
  }

  // Joystick 2 - Y (Pince/Grip)
  // Les SG90 sont souvent utilisés pour une pince: 0° fermé, 90° ou 180° ouvert.
  if (valJoy2_Y < 400) {
    posPince = min(180, posPince + 2); // Ouvrir la pince
  } else if (valJoy2_Y > 600) {
    posPince = max(0, posPince - 2); // Fermer la pince
  }
  
  // Mettre à jour les Servomoteurs
  servoBase.write(posBase);
  servoEpaule.write(posEpaule);
  servoCoude.write(posCoude);
  servoPince.write(posPince);

  delay(15); // Petite pause pour lissage et éviter la surcharge CPU
}

//Séquence Automatique (autoSequence())
//C'est ici que vous programmez la suite de mouvements (séquence) que le bras doit effectuer en mode automatique.
void autoSequence() {
  // Exemple de séquence simple : Aller à un point, attendre, revenir au point de départ.
  
  // Point A (Prise)
  moveArmTo(10, 120, 30, 180, 1000); // Base, Epaule, Coude, Pince (ouvert), délai

  // Fermer la pince (prise)
  moveArmTo(10, 120, 30, 10, 500); // Pince (fermé)

  // Point B (Transfert)
  moveArmTo(150, 60, 150, 10, 1500); // Base, Epaule, Coude, Pince (fermé)

  // Ouvrir la pince (lâcher)
  moveArmTo(150, 60, 150, 180, 500); // Pince (ouvert)

  // Retour à la position de repos
  moveArmTo(90, 90, 90, 10, 1000); 

  // Option : Revenir en mode manuel après la séquence
  // modeRobot = 0; 
}

// Fonction utilitaire pour bouger tous les servos simultanément
void moveArmTo(int base, int epaule, int coude, int pince, int delayTime) {
  // Mettre à jour les positions globales (pour un éventuel retour au mode manuel)
  posBase = base;
  posEpaule = epaule;
  posCoude = coude;
  posPince = pince;

  // Écrire les nouvelles positions
  servoBase.write(base);
  servoEpaule.write(epaule);
  servoCoude.write(coude);
  servoPince.write(pince);

  delay(delayTime);
}


void setup() {
  
  Serial.begin(9600);
  
  // Attacher les servomoteurs aux broches
  servoBase.attach(SERVO_BASE_PIN);
  servoEpaule.attach(SERVO_EPAULE_PIN);
  servoCoude.attach(SERVO_COUDE_PIN);
  servoPince.attach(SERVO_PINCE_PIN);

  // Initialiser les positions des servos
  servoBase.write(posBase);
  servoEpaule.write(posEpaule);
  servoCoude.write(posCoude);
  servoPince.write(posPince);

  // Initialiser les broches 
  pinMode(JOY1_SW_PIN, INPUT_PULLUP); 
  pinMode(JOY2_SW_PIN, INPUT_PULLUP);
  pinMode(buttonPin, INPUT_PULLUP);
  
  pinMode(LEDPIN_MANU, OUTPUT);
  pinMode(LEDPIN_AUTO, OUTPUT); 

  // indiquez à l'objet bouton quelle broche lire
  button.attach(buttonPin);
  // exécutez la première sous-routine lorsque l'état est défini sur false
  // deuxième sous-routine lorsque l'état est défini sur true
  state = false;

  Serial.println("Initialisation du bras robotise terminee.");
 
  delay(1500); // Petite pause pour affichage
}

void loop() {
  // vérifiez si le bouton est enfoncé
  if(getButton()){
    // si vrai, inversez l'état
    state = !state;
     }
  // lancez l'une ou l'autre des sous-routines en fonction de l'état   
 switch(state) {
      case false:      
        subRoutineOne();       
        break;
      case true:
        subRoutineTwo();   
        break;      
    }   
}
// la routine pour vérifier l'appui sur le bouton
bool getButton(){
  if(button.update() && button.read() == LOW){    
    return true;
  } else {
    return false;
  }
}

// subRoutineOne
void subRoutineOne(){
  digitalWrite(LEDPIN_MANU,HIGH); 
  digitalWrite(LEDPIN_AUTO,LOW);
  manuelControl();
}

// subRoutineTwo
void subRoutineTwo(){
  digitalWrite(LEDPIN_MANU,LOW); 
  digitalWrite(LEDPIN_AUTO,HIGH);
  autoSequence();
}