/*
    ***********************************************
    *  Gestion de 3 aiguillages par servomoteur   *
    *              version 20230709               *
    *             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  !
  !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

*/

#include <Servo.h>
const int nbServos = 1;
Servo servo[nbServos];

const int pinButtons[] = {A0};
const int pinServos[] = {8};

int angleIntermediaire[nbServos];
int angleCible[nbServos];
int precedentePosition[nbServos];
int sens[nbServos]; 
const int temporisation = 10;
int compteurLoop;

unsigned long chronoMouvement[nbServos];
unsigned long chronoAttach[nbServos];
unsigned long chronoDetach[nbServos];

void setup() {
  Serial.begin(9600);

  for (int i = 0; i < nbServos; i++) {  
    pinMode(pinServos[i], OUTPUT);
    angleIntermediaire[i] = map(analogRead(pinServos[i]),0,1023,544,2400);
    precedentePosition[i] = angleIntermediaire[i];
    
    if (!servo[i].attached()) {servo[i].attach(pinServos[i]);}
    delay(50);
    servo[i].writeMicroseconds(angleIntermediaire[i]);
    delay(500);
    
    if (servo[i].attached()) {servo[i].detach();}
    delay(50);
    chronoMouvement[i] = millis();
    chronoAttach[i] = millis();
    chronoDetach[i] = millis();
  } // Fin de for (int i = 0; i < nbServos; i++)
} // Fin de setup

void loop() {
  angleCible[compteurLoop] = map(analogRead(pinServos[compteurLoop]),0,1023,544,2400);
  if (angleCible[compteurLoop]!= precedentePosition[compteurLoop]){
    //angleIntermediaire[compteurLoop] = precedentePosition[compteurLoop];
    
  	if (angleCible[compteurLoop]> precedentePosition[compteurLoop]){
      sens[compteurLoop] = 1;
    } else {
      sens[compteurLoop] = -1;
    } // Fin de if (angleCible[compteurLoop]> precedentePosition[compteurLoop])
    
    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(angleIntermediaire[compteurLoop] == angleCible[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)precedentePosition[compteurLoop] = angleCible[compteurLoop]
     	precedentePosition[compteurLoop] = angleCible[compteurLoop];
    } else {
      	Serial.print(angleIntermediaire[compteurLoop]);
    	Serial.print(F(" => "));
      	Serial.println(angleCible[compteurLoop]);  	
      
        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]){
  } // Fin de if (angleIntermediaire[compteurLoop]!= precedentePosition[compteurLoop])
  
  compteurLoop +=1;
  if (compteurLoop == nbServos){compteurLoop = 0;}
}