// PONT_TOURNANT.ino
// ----------------------------------------------------------------------------------
// Christian BEZANGER - décembre 2023 - Version 7 pour carte MEGA2560
// Ce programme est dans le domaine public
// ----------------------------------------------------------------------------------
// COMMANDES :
// 'A' AHEAD AVANCE, 'B' BACK RECULE, 'C' CALM STOP
// 'D' 256 PAS CW, '0' 16 PAS CW, '*' 1 PAS CW, '#' 1 PAS CCW
// '1' a '5' CHOIX DE LA VOIE
// ----------------------------------------------------------------------------------
// MONTAGE :
// Interface moteur A0 - A3
// Clavier D4 - D11
// Carte relais 2 RT D2 Etat HIGH - Polarité sur V0 (pont tournant)
// Carte relais 2 RT D3 Etat HIGH - Polarité sur V1 (voie d'entrée ou de sortie)
// Carte relais 1RT D23 - D33 (impair) Etat LOW -Connexion voies 2 à 5
// Ecran OLED en I2C (facultatif)
// ----------------------------------------------------------------------------------

#include <Keypad.h>             // version 3.1.1
#include <Wire.h>               
#include <Adafruit_GFX.h>       // version 1.11.7
#include <Adafruit_SSD1306.h>   // Version 2.5.7

#define SCREEN_WIDTH 128 // largeur en pixels
#define SCREEN_HEIGHT 64 // hauteur en pixels

// Declaration de l'ecran SSD1306 connecte en I2C (SDA, SCL pins)
// --------------------------------------------------------------
#define OLED_RESET     -1 // Reset pin # ou -1 si partage RESET Arduino
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// Definition des symboles des touches du clavier 
// ----------------------------------------------
char keys[4][4] = {
  {'1', '2', '3', 'A'},
  {'4', '5', '6', 'B'},
  {'7', '8', '9', 'C'},
  {'*', '0', '#', 'D'}
};

// Connexions du clavier
// ---------------------
byte rowPins[4] = {11, 10, 9, 8}; // pin des rangees du clavier
byte colPins[4] = {7, 6, 5, 4};   // pin des colonnes du clavier

// initialise instance de classe pour objet clavier
// ------------------------------------------------
Keypad myKeypad = Keypad(makeKeymap(keys), rowPins, colPins, 4, 4);

// Moteur en demi-pas
// ------------------
byte valeurPhase[] = {1, 3, 2, 6, 4, 12, 8, 9};
static int index = 0;  // index valeurPhase de 0 a 7

// Sorties de commande des relais d'alimentation de voie
// -----------------------------------------------------
byte relaisVoie[] = {23, 25, 27, 29, 31, 33};

// Variables
// ---------
byte numCible;  // 0 a 15 possible
byte OLDnumCible;
byte numVoie;   // 1 a 5 possible
int Deplacement = 0;
int positionPont = 0;  // De 0 (voie 0) a 4095
bool dir;  // true = CCW, false = CW
byte sensMarche;  // 0 = ARRET, 1 = AVANT, 2 = ARRIERE
bool flagPrint = true;  // Flag pour impression unique message d'avertissement

void movePont(bool dir, int steps, byte ms) {
  for (int i = 0; i <= 5; i++) {
      digitalWrite(relaisVoie[i], HIGH);  // Arret voie pont et toutes voies externes
    }
  sensMarche = 0;
  digitalWrite(2, LOW);  // Configuration normale
  digitalWrite(3, LOW);  // Configuration normale
  display.clearDisplay();
  display.setCursor(10,28);
  display.print("Attendre");
  display.display();
  moveSteps(dir, steps, ms);  // Mouvement du pont
  Serial.print("Voie ");
  Serial.print(numVoie);
  Serial.println(" OK");
  display.clearDisplay();
  display.setCursor(10,28);
	display.print("VOIE ");
  display.print(numVoie);
  display.print(" OK");
  display.display();
}

void moveSteps(bool dir, int steps, byte ms) {
  for (int i = 0; i < steps; i++) {
    moveOneStep(dir); // Tourne d'un demi-pas
    delay(ms);        // Controle la vitesse (duree entre chaque demi-pas)
  }
}

void moveOneStep(bool dir) {
  if (dir) {  // CCW
    PORTF = valeurPhase[index];
    index = index + 1;
    if(index == 8) {
      index = 0;
    }
  }
  else {      // CW
    PORTF = valeurPhase[index];
    index = index - 1;
    if(index == -1) {
      index = 7;
    }
  }
}

void setup() {
  // initialise avec adresse I2C 0x3C (a changer si differente)
  // ----------------------------------------------------------
	display.begin(SSD1306_SWITCHCAPVCC, 0x3C);  

	// Effacement de la page, taille et couleur texte
  // ----------------------------------------------
	display.clearDisplay();
  display.display();
  display.setTextSize(2);
	display.setTextColor(WHITE);

  Serial.begin(9600); // Initialise la transmission vers le moniteur serie
  pinMode(2, OUTPUT); // Relais de polarité sur V0 (pont tournant)
  pinMode(3, OUTPUT); // Relais de polarité sur V1 (voie d'entrée)

  // PORTF en OUTPUT (sorties 54 a 61)
  // ---------------------------------
  for(int i = 54; i <= 61; i++) {
    pinMode(i, OUTPUT);
  }

  // Sorties commandant relais de voies en OUTPUT et HIGH (relais OFF)
  // -----------------------------------------------------------------
  for(int j = 0; j <= 5; j++) {
    pinMode(relaisVoie[j], OUTPUT);
    digitalWrite(relaisVoie[j], HIGH);
    delay(500);
  }

  PORTF = valeurPhase[index];
  numCible = 0;    // Initialisation du pont a la cible 0 (voie entree)
  OLDnumCible = 0;
  numVoie = 1;     // Initialisation du pont voie 1 en entree
  sensMarche = 0;  // Initialisation ARRET
  Serial.println("Le pont est initialise !");
	display.setCursor(10,28);
	display.println("INIT : OK");
  display.display();
}

void loop() {
  // Lecture du caractere tape sur le clavier
  // ----------------------------------------
  char keyPressed = myKeypad.getKey();
  // Transformation voie en cible pour le pont
  // -----------------------------------------
  switch (keyPressed) {
    case '0':
    // Le pont se deplace de 16 pas CW
    movePont(false, 16, 10);
    Serial.println("16 PAS CW");
    display.clearDisplay();
    display.setCursor(10,28);
    display.println("16 PAS CW");
    display.display();
    break;
    case '1':
    // Voie selectionnee 1 - Cible 0
    numVoie = 1;
    numCible = 0;
    break;
    case '2':
    // Voie selectionnee 2 - Cible 7
    numVoie = 2;
    numCible = 7;
    break;
    case '3':
    // Voie selectionnee 3 - Cible 8
    numVoie = 3;
    numCible = 8;
    break;
    case '4':
    // Voie selectionnee 4 - Cible 9
    numVoie = 4;
    numCible = 9;
    break;
    case '5':
    // Voie selectionnee 5 - Cible = 10
    numVoie = 5;
    numCible = 10;
    break;
    case 'A':
    // Avance
    sensMarche = 1;
    Serial.println("AVANCE");
    display.clearDisplay();
    display.setCursor(10,28);
    display.println("AVANCE");
    display.display();
    break;
    case 'B':
    // Recule
    sensMarche = 2;
    Serial.println("RECULE");
    display.clearDisplay();
    display.setCursor(10,28);
    display.println("RECULE");
    display.display();
    break;
    case 'C':
    // Arrete
    sensMarche = 0;
    Serial.println("ARRET");
    display.clearDisplay();
    display.setCursor(10,28);
    display.println("ARRET");
    display.display();
    break;
    case 'D':
    // Le pont se deplace de 256 pas CW
    movePont(false, 256, 10);
    Serial.println("256 PAS CW");
    display.clearDisplay();
    display.setCursor(3,28);
	  display.println("256 PAS CW");
    display.display();
    break;
    case '*':
    // Le pont se deplace d'un pas CW
    movePont(false, 1, 10);
    Serial.println("1 PAS CW");
    display.clearDisplay();
    display.setCursor(10,28);
	  display.println("1 PAS CW");
    display.display();
    break;
    case '#':
    // Le pont se deplace d'un pas CCW
    movePont(true, 1, 10);
    Serial.println("1 PAS CCW");
    display.clearDisplay();
    display.setCursor(10,28);
	  display.println("1 PAS CCW");
    display.display();
    break;
    default:
    break;
  }  

  // Deplacement du pont si necessaire
  // ---------------------------------
  if(numCible != OLDnumCible) {
      Deplacement = (numCible - OLDnumCible)*256;
      if(Deplacement > 0) {
        dir = false;
    } else {  
        dir = true;
    }
    positionPont = ((positionPont + Deplacement) % 4096);
    Deplacement = abs(Deplacement);
    movePont(dir, Deplacement, 10);
    OLDnumCible = numCible;
    Serial.print("Position du pont ");
    Serial.println(positionPont);
  }

  // Gestion du sens de marche (sens du courant sur feeder)
  // ------------------------------------------------------
  if(sensMarche == 1) {digitalWrite(2, LOW);}   // Relais 2RT RA non sollicité
  if(sensMarche == 2) {digitalWrite(2, HIGH);}  // Relais 2RT RA sollicité

  // Alimentation des voies concernees (voie 0 et voie selectionnee)
  // ---------------------------------------------------------------
  if(sensMarche == 0) {
    for (int i = 0; i <= 5; i++) {
      digitalWrite(relaisVoie[i], HIGH);  // Arret voie pont et toutes voies externes
    }
    digitalWrite(2, LOW);  // Configuration normale
    digitalWrite(3, LOW);  // Configuration normale
  }
  if(sensMarche == 1) {digitalWrite(relaisVoie[0], LOW); digitalWrite(relaisVoie[numVoie], LOW);}
  if(sensMarche == 2) {digitalWrite(relaisVoie[0], LOW); digitalWrite(relaisVoie[numVoie], LOW);}
  
  // Si voie 3 : polarité identique voies 0, 1 et 3
  // ----------------------------------------------
  if((numVoie == 3) && (sensMarche == 1)) {
    digitalWrite(3, HIGH);  // Inversion polarité voie 1 pour sortie en marche avant
    digitalWrite(relaisVoie[1], LOW);  // Alimente également voie 1
  }
  if((numVoie == 3) && (sensMarche == 2)) {
    digitalWrite(3, LOW);  // Inversion polarité voie 1 pour manoeuvre en marche arrièere
    digitalWrite(relaisVoie[1], LOW);  // Alimente également voie 1
  }
}