/*
    ***********************************************
    *  Gestion de 5 aiguillages par servomoteur   *
    *              version 20230708               *
    *   Utilisation de la bibliothèque Bounce2    *
    *             Arduino UNO                     *
    *                                             *
    ***********************************************

      
  !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  !            Ce code est distribué gratuitement sous licence              !
  !                 Creativ Commons CC-BY-NC-ND 3.0                         !
  !                                                                         !
  !   Cette licence permet à d'autres utilisateurs d'utiliser ce code       !
  ! à des fins non commerciales,  dans la mesure où le nom de l'auteur est  !
  ! mentionné. Toute modification du code devra être soumise à              !
  ! l'autorisation expresse de l'auteur.                                    !
  !                                                                         !
  ! auteur Philippe GUENET - [email protected] - https://wgnt-train.fr  !
  !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

  
  - Les valeurs des angles sont à déterminer matériellement. Il suffira de renseigner
    les tableaux angleDirect[] et angleDevie[].
  - Possibilité de valeurs différentes pour chaque servomoteur car il est très difficile 
    matériellement de positionner les servos.
  - L'application accepte que les moteurs de servomoteurs puissent être installés 
    dans n'importe quel sens. (Pas d'obligation que angleDirect soit plus petit que 
    angleDevie et inversement.)
  - Pas de fonctions blocantes ni de boucles dans le loop. Plusieurs servomoteurs 
    peuvent donc fonctionner en même temps.
  - Avec un arduino Nano, on pourrait ajouter un 6ème serrvomoteur.
  */

#include <Servo.h>
#include <Bounce2.h>

// Constantes pour les pins
const int nbServos = 5;
int compteurLoop;
const int pinButtons[] = {3, 4, 5, 6, 7};
const int pinServos[] = {8, 9, 10, 11, 12};
Bounce BPServo[nbServos] = Bounce();
const int dureeAntiRebond = 15;

Servo servo[nbServos];

const int angleDirect[] = {1300, 1700, 1300, 1300, 1300};
const int angleDevie[] = {1700, 1300, 1700, 1700, 1700};
int angleIntermediaire[nbServos];
int sens[nbServos]; 
const int temporisation = 10;
unsigned long chronoMouvement[nbServos];
unsigned long chronoAttach[nbServos];
unsigned long chronoDetach[nbServos];

enum aiguillages {
      DIRECT_DEVIE,
      DIRECT,
      DEVIE_DIRECT,
      DEVIE
    };
    aiguillages etatAiguillage[nbServos];

void setup() {
  for (int i = 0; i < nbServos; i++) {
    
    /* Initialisation des boutons poussoirs */
    pinMode(pinButtons[i], INPUT);
    pinMode(pinServos[i], OUTPUT);
    BPServo[i].attach(pinButtons[i], INPUT);
    BPServo[i].interval(dureeAntiRebond);
    /* ==================================== */

    delay(50);
    if (digitalRead(pinButtons[i]) == HIGH) {
      angleIntermediaire[i] = angleDevie[i];
      etatAiguillage[i] = DEVIE;
    } else {
      angleIntermediaire[i] = angleDirect[i];
      etatAiguillage[i] = DIRECT; 
    } // Fin de if (digitalRead(pinButtons[i]) == HIGH)

    BPServo[i].update();

    /* Initialisation des servomoteurs */
    if (!servo[i].attached()) {servo[i].attach(pinServos[i]);}
    delay(50);  
    servo[i].writeMicroseconds(angleIntermediaire[i]);
    delay(300);
    if (servo[i].attached()) {servo[i].detach();}
    delay(50);
    /* =============================== */ 

    if (angleDirect[i] < angleDevie[i]){
      sens[i] = 1;
    } else {
      sens[i] = -1;
    }

    chronoMouvement[i] = millis();
    chronoAttach[i] = millis();
    chronoDetach[i] = millis();
  } // Fin de for (int i = 0; i < nbServos; i++)
} // Fin de setup

void loop() {
  BPServo[compteurLoop].update();
  if (BPServo[compteurLoop].changed()){
     if (millis() - chronoAttach[compteurLoop] >= 50) {
      // on branche le servo
      if (!servo[compteurLoop].attached()) {servo[compteurLoop].attach(pinServos[compteurLoop]);}
      chronoAttach[compteurLoop] = millis();
    } // Fin de if (millis() - chronoAttach[compteurLoop] >= 50)
 
    if (BPServo[compteurLoop].read() == HIGH){
      etatAiguillage[compteurLoop] = DIRECT_DEVIE;
    } else {
      etatAiguillage[compteurLoop] = DEVIE_DIRECT;
    } // Fin de if (BPServo[compteurLoop].read() == HIGH){
  } // Fin de if (BPServo[compteurLoop].changed())

 switch (etatAiguillage[compteurLoop]) {
    case DIRECT :
      angleIntermediaire[compteurLoop] = angleDirect[compteurLoop];

      break;
      
    case DIRECT_DEVIE :        
      if(angleIntermediaire[compteurLoop] == angleDevie[compteurLoop]){
        if (millis() - chronoDetach[compteurLoop] >= 50) {
          // on débranche le servo
          if (servo[compteurLoop].attached()) {servo[compteurLoop].detach();}
          chronoDetach[compteurLoop] = millis();
        } // Fin de if (millis() - chronoDetach[compteurLoop] >= 50)
        etatAiguillage[compteurLoop] = DEVIE;
      } else {
        if (millis() - chronoMouvement[compteurLoop] >= temporisation) {
          angleIntermediaire[compteurLoop] += sens[compteurLoop];
          servo[compteurLoop].writeMicroseconds(angleIntermediaire[compteurLoop]);
          chronoMouvement[compteurLoop] = millis();
        } // Fin de if (millis() - chronoMouvement[compteurLoop] >= temporisation)
     } // Fin de if(angleIntermediaire[compteurLoop] == angleDevie[compteurLoop]){

      break;

    case DEVIE :
      angleIntermediaire[compteurLoop] = angleDevie[compteurLoop];

      break;
        
    case DEVIE_DIRECT :
      if(angleIntermediaire[compteurLoop] == angleDirect[compteurLoop]){
        if (millis() - chronoDetach[compteurLoop] >= 50) {
          // on débranche le servo
          if (servo[compteurLoop].attached()) {servo[compteurLoop].detach();}
          chronoDetach[compteurLoop] = millis();
        } // Fin de if (millis() - chronoDetach[compteurLoop] >= 50)
        etatAiguillage[compteurLoop] = DIRECT;
      } else {
        if (millis() - chronoMouvement[compteurLoop] >= temporisation) {
          angleIntermediaire[compteurLoop] -= sens[compteurLoop];
          servo[compteurLoop].writeMicroseconds(angleIntermediaire[compteurLoop]);
          chronoMouvement[compteurLoop] = millis();
        } // Fin de if (millis() - chronoMouvement[compteurLoop] >= temporisation)
      } // Fin de if(angleIntermediaire[compteurLoop] == angleDirect[compteurLoop])
               
      break;

      
      } // Fin de switch (etatAiguillage[compteurLoop])
  compteurLoop +=1;
  if (compteurLoop == nbServos){compteurLoop = 0;}
} // Fin de loop