/*
    Name:       AF_boissellerie_MoteurVisAbille.ino
    Created:	28.08.2024
    Author:     BUREAU\admin
	Remarque:	Comande d'un moteur pas à pas et vis à bille pour tiroir.
				https://forum.arduino.cc/t/aide-sur-gestion-moteur-pas-a-pas-avec-vis-sans-fin-et-ecrou-a-bille/1295712/11
	01.09.2024	Première version #15
*/

#include <AccelStepper.h>  // https://www.airspayce.com/mikem/arduino/AccelStepper/classAccelStepper.html

//------------------------------------- Moteur pas à pas (mpap)
// Définir les connexions du moteurs pas à pas droit
const int  mpap1DirPin = 2; // Pin qui commande la direction
const int  mpap1StepPin = 4; // Pin qui commande les pas
const int  mpap1EnablePin = 10; // Pin d'activation du mpap
const long mpap1TargetPosition =  57060;  // Définir la position cible (-1500 pas / -37.5mm)

// Définir les connexions du moteurs pas à pas gauche
const int  mpap2DirPin = 5; // Pin qui commande la direction
const int  mpap2StepPin = 6; // Pin qui commande les pas
const int  mpap2EnablePin = 12; // Pin d'activation du mpap
const long mpap2TargetPosition =  57060;  // Définir la position cible (-1500 pas / -37.5mm)

AccelStepper mpap1(1, mpap1StepPin, mpap1DirPin);  // Créer une nouvelle instance de la classe AccelStepper = moteur droit
AccelStepper mpap2(1, mpap2StepPin, mpap2DirPin);  // Créer une nouvelle instance de la classe AccelStepper = moteur gauche

const boolean mpap1BitInvertDir = true;  // Pour inverser le bit DIR
const boolean mpap1BitInvertStep = true;  // Pour inverser le bit STEP
const boolean mpap1BitInvertEna = true;  // Pour inverser le bit ENA

const boolean mpap2BitInvertDir = true;  // Pour inverser le bit DIR
const boolean mpap2BitInvertStep = true;  // Pour inverser le bit STEP
const boolean mpap2BitInvertEna = true;  // Pour inverser le bit ENA

enum {mpap1DirToHome, mpap1DirToTarget};
int mpap1DirTo = mpap1DirToTarget;

enum {mpap2DirToHome, mpap2DirToTarget};
int mpap2DirTo = mpap2DirToTarget;

long mpap1StepsPosition = 0;  // En cas d'arrêt en cours, position en poas moteur droit
long mpap2StepsPosition = 0;  // En cas d'arrêt en cours, position en poas moteur gauche

//------------------------------------- Endstop vis à bille (vab)
// Définir les pins des capteurs de fin de course droits
const int vab1EndstopHomePin = A0;  // Contact de départ
const int vab1EndstopTargetPin = A1; // Contact d^'arrivée
const int vab1EndstopEtatOn = LOW; // Etat quand actif

// Définir les pins des capteurs de fin de course gauches
const int vab2EndstopHomePin = A2;  // Contact de départ
const int vab2EndstopTargetPin = A3; // Contact d^'arrivée
const int vab2EndstopEtatOn = LOW; // Etat quand actif

//------------------------------------- Boutons (btn)
// Définir les pins des interrupteurs biposition droit
const int btn1SortiePin = 11; // 6 // Pin de l'interrupteur Sortie
const int btn1EntreePin = 3; // 7 // Pin de l'interrupteur Entrée
const int btn1EtatOn = LOW; // Etat quand actif

// Définir les pins des interrupteurs biposition gauche
const int btn2SortiePin = 7; // 6 // Pin de l'interrupteur Sortie
const int btn2EntreePin = 8; // 7 // Pin de l'interrupteur Entrée
const int btn2EtatOn = LOW; // Etat quand actif

//------------------------------------- Etats du programme droit
const String prg1EtatLabel[] = {"Initial", "en Marche", "en Pause"};
enum {prg1EtatInitial, prg1EtatEnMarche, prg1EtatEnPause};
int prg1Etat;  // Etat du programme droit

//------------------------------------- Etats du programme gauche
const String prg2EtatLabel[] = {"Initial", "en Marche", "en Pause"};
enum {prg2EtatInitial, prg2EtatEnMarche, prg2EtatEnPause};
int prg2Etat;  // Etat du programme gauche

void setup()
{
	Serial.begin(115200);
	delay(500);
	Serial.println("\nMoteur pas a pas avec vis a billes");

	mpap1.setMaxSpeed(3000); // Définir la vitesse maximale moteur droit 
	mpap1.setAcceleration(700); // Définir l'accélération moteur droit 

  mpap2.setMaxSpeed(3000); // Définir la vitesse maximale moteur gauche
	mpap2.setAcceleration(700); // Définir l'accélération moteur gauche
	
	mpap1.setEnablePin(mpap1EnablePin);
	//                    DIR                      STEP               ENA
	mpap1.setPinsInverted(!mpap1BitInvertDir, !mpap1BitInvertStep, mpap1BitInvertEna);
	
	pinMode(vab1EndstopHomePin, INPUT_PULLUP);
	pinMode(vab1EndstopTargetPin, INPUT_PULLUP);
	
	pinMode(btn1SortiePin, INPUT_PULLUP);
	pinMode(btn1EntreePin, INPUT_PULLUP);
	
	mpap1.disableOutputs();

	mpap2.setEnablePin(mpap2EnablePin);
	//                    DIR                      STEP               ENA
	mpap2.setPinsInverted(!mpap2BitInvertDir, !mpap2BitInvertStep, mpap2BitInvertEna);
	
	pinMode(vab2EndstopHomePin, INPUT_PULLUP);
	pinMode(vab2EndstopTargetPin, INPUT_PULLUP);
	
	pinMode(btn2SortiePin, INPUT_PULLUP);
	pinMode(btn2EntreePin, INPUT_PULLUP);
	
	mpap2.disableOutputs();

	//toolMonitor(); // Pour afficher l'état des ports E/S

	prg1EtatChange(prg1EtatEnPause);
	mpap1DirTo = mpap1DirToTarget;
	mpap1ToEndstopHome();

  prg2EtatChange(prg2EtatEnPause);
	mpap2DirTo = mpap2DirToTarget;
	mpap2ToEndstopHome();
}

void loop()
{
	if (prg1Etat == prg1EtatEnPause)
	{
		if (digitalRead(btn1SortiePin) == btn1EtatOn) // Si le bouton sortie est pressé
		{
			if (digitalRead(vab1EndstopTargetPin) == !vab1EndstopEtatOn)  // Si pas sur contact arrivée
			{
				Serial.println(F("Bouton 1 SORTIE"));
				prg1EtatChange(prg1EtatEnMarche);

				mpap1DirTo = mpap1DirToTarget;
				mpap1.move(mpap1TargetPosition - mpap1StepsPosition);
			}
		} 
		else if (digitalRead(btn1EntreePin) == btn1EtatOn) // Si le bouton entrée est pressé
		{
			if (digitalRead(vab1EndstopHomePin) == !vab1EndstopEtatOn)  // Si pas sur contact départ
			{
				Serial.println(F("Bouton 1 ENTREE"));
				prg1EtatChange(prg1EtatEnMarche);

				mpap1DirTo = mpap1DirToHome;
				mpap1.move(-mpap1StepsPosition);
			}
		}
	}

if (prg2Etat == prg2EtatEnPause)
	{
		if (digitalRead(btn2SortiePin) == btn2EtatOn) // Si le bouton sortie est pressé
		{
			if (digitalRead(vab2EndstopTargetPin) == !vab2EndstopEtatOn)  // Si pas sur contact arrivée
			{
				Serial.println(F("Bouton 2 SORTIE"));
				prg2EtatChange(prg2EtatEnMarche);

				mpap2DirTo = mpap2DirToTarget;
				mpap2.move(mpap2TargetPosition - mpap2StepsPosition);
			}
		} 
		else if (digitalRead(btn2EntreePin) == btn2EtatOn) // Si le bouton entrée est pressé
		{
			if (digitalRead(vab2EndstopHomePin) == !vab2EndstopEtatOn)  // Si pas sur contact départ
			{
				Serial.println(F("Bouton 2 ENTREE"));
				prg2EtatChange(prg2EtatEnMarche);

				mpap2DirTo = mpap2DirToHome;
				mpap2.move(-mpap2StepsPosition);
			}
		}
	}

	if ((digitalRead(btn1SortiePin) != btn1EtatOn && digitalRead(btn1EntreePin) != btn1EtatOn) && prg1Etat == prg1EtatEnMarche) // Si le bouton sortie et entrée sont relâchés
	{
		Serial.println(F("Boutons 1 STOP"));
		mpap1Stop(); // Arrêt du moteur
		prg1EtatChange(prg1EtatEnPause);
	}

	if (prg1Etat == prg1EtatEnMarche) // Faire tourner le moteur, si nécessaire
	{
		mpap1.run();
	}

if ((digitalRead(btn2SortiePin) != btn2EtatOn && digitalRead(btn2EntreePin) != btn2EtatOn) && prg2Etat == prg2EtatEnMarche) // Si le bouton sortie et entrée sont relâchés
	{
		Serial.println(F("Boutons 2 STOP"));
		mpap2Stop(); // Arrêt du moteur
		prg2EtatChange(prg2EtatEnPause);
	}

	if (prg2Etat == prg2EtatEnMarche) // Faire tourner le moteur, si nécessaire
	{
		mpap2.run();
	}

	//--------------------------------- Si moteur droit en fin de course
	if ((digitalRead(vab1EndstopHomePin) == vab1EndstopEtatOn) && mpap1DirTo == mpap1DirToHome && prg1Etat == prg1EtatEnMarche) // Si moteur déjà au départ
	{
		mpap1StepsPosition = 0;
		mpap1Stop();
	}

	if ((digitalRead(vab1EndstopTargetPin) == vab1EndstopEtatOn) && mpap1DirTo == mpap1DirToTarget && prg1Etat == prg1EtatEnMarche) // Si moteur déjà aun target
	{
		mpap1StepsPosition = mpap1TargetPosition;
		mpap1Stop();
	}

  //--------------------------------- Si moteur gauche en fin de course
	if ((digitalRead(vab2EndstopHomePin) == vab2EndstopEtatOn) && mpap2DirTo == mpap2DirToHome && prg2Etat == prg2EtatEnMarche) // Si moteur déjà au départ
	{
		mpap2StepsPosition = 0;
		mpap2Stop();
	}

	if ((digitalRead(vab2EndstopTargetPin) == vab2EndstopEtatOn) && mpap2DirTo == mpap2DirToTarget && prg2Etat == prg2EtatEnMarche) // Si moteur déjà aun target
	{
		mpap2StepsPosition = mpap2TargetPosition;
		mpap2Stop();
	}
  
}

//------------------------------------- Etats du programme 1
void prg1EtatChange(int etatNew)
{
	Serial.println(F("Changement d'etat côté droit"));
	Serial.println("\tde " + prg1EtatLabel[prg1Etat]);

	Serial.println("\ta " + prg1EtatLabel[etatNew]);

	prg1Etat = etatNew;

	if (prg1Etat == prg1EtatEnPause)
	{
		mpap1.disableOutputs();
	}
	else if (prg1Etat == prg1EtatEnMarche)
	{
		mpap1.enableOutputs();
	}
}

//------------------------------------- Etats du programme 
void prg2EtatChange(int etatNew)
{
	Serial.println(F("Changement d'etat côté gauche"));
	Serial.println("\tde " + prg2EtatLabel[prg2Etat]);

	Serial.println("\ta " + prg2EtatLabel[etatNew]);

	prg2Etat = etatNew;

	if (prg2Etat == prg2EtatEnPause)
	{
		mpap2.disableOutputs();
	}
	else if (prg2Etat == prg1EtatEnMarche)
	{
		mpap2.enableOutputs();
	}
}


//------------------------------------ Arrêt du moteur droit
void mpap1Stop()
{
	if (mpap1DirTo == mpap1DirToTarget)
	{
		mpap1StepsPosition += mpap1.currentPosition();
	} 
	else
	{
		mpap1StepsPosition += mpap1.currentPosition();
	}
		mpap1.setCurrentPosition(0);
		mpap1.stop();
}

//------------------------------------ Arrêt du moteur gauche
void mpap2Stop()
{
	if (mpap2DirTo == mpap2DirToTarget)
	{
		mpap2StepsPosition += mpap2.currentPosition();
	} 
	else
	{
		mpap2StepsPosition += mpap2.currentPosition();
	}
		mpap2.setCurrentPosition(0);
		mpap2.stop();
}

//------------------------------------ Moteur en position endstopHome moteur droit
void mpap1ToEndstopHome()
{
	if (digitalRead(vab1EndstopHomePin) != vab1EndstopEtatOn)     // Si moteur pas sur contact.
	{
		float savedMaxSpeed1 = mpap1.maxSpeed();  // Pour sauver la vitesse actuelle
		mpap1.setMaxSpeed(500);  // Réduire la vitesse

		prg1EtatChange(prg1EtatEnMarche);
		Serial.println(F("\tMoteur droit vers endstopHome "));
		mpap1.setCurrentPosition(0);     // Moteur position 0
	
		mpap1.moveTo(-mpap1TargetPosition * 2);

		while (true)
		{
			if (digitalRead(vab1EndstopHomePin) == vab1EndstopEtatOn)     // Si moteur sur contact.
			{
				mpap1.setCurrentPosition(0);     // Moteur position 0
				mpap1.stop();
				break;
			}
			mpap1.run();
		}
		prg1EtatChange(prg1EtatEnPause);
		mpap1.setMaxSpeed(savedMaxSpeed1);  // Restituer la vitesse
	}
	mpap1DirTo = mpap1DirToTarget;  // Prochaine direction

	mpap1StepsPosition = 0;
	Serial.println(F("\tMoteur droit sur endstopHome "));
}

//------------------------------------ Moteur en position endstopHome moteur gauche
void mpap2ToEndstopHome()
{
	if (digitalRead(vab2EndstopHomePin) != vab2EndstopEtatOn)     // Si moteur pas sur contact.
	{
		float savedMaxSpeed2 = mpap2.maxSpeed();  // Pour sauver la vitesse actuelle
		mpap2.setMaxSpeed(500);  // Réduire la vitesse

		prg2EtatChange(prg2EtatEnMarche);
		Serial.println(F("\tMoteur gauche vers endstopHome "));
		mpap2.setCurrentPosition(0);     // Moteur position 0
	
		mpap2.moveTo(-mpap2TargetPosition * 2);

		while (true)
		{
			if (digitalRead(vab2EndstopHomePin) == vab2EndstopEtatOn)     // Si moteur sur contact.
			{
				mpap2.setCurrentPosition(0);     // Moteur position 0
				mpap2.stop();
				break;
			}
			mpap2.run();
		}
		prg2EtatChange(prg2EtatEnPause);
		mpap2.setMaxSpeed(savedMaxSpeed2);  // Restituer la vitesse
	}
	mpap2DirTo = mpap2DirToTarget;  // Prochaine direction

	mpap2StepsPosition = 0;
	Serial.println(F("\tMoteur gauche sur endstopHome "));
}

/*
    Moniteur des E/S
	Attention, reste dans cette boucle.
	Pour arrêter, commenter la ligne:
			//toolMonitor(); // Pour afficher l'état des ports E/S
*/
//------------------------------------- Moniteur des E/S 
void toolMonitor()
{
	while (true)
	{
		Serial.print(F("\nEndstop HOME\t")); Serial.println((digitalRead(vab1EndstopHomePin) == vab1EndstopEtatOn) ? "ON" : "OFF");
		Serial.print(F("Endstop TARGET\t")); Serial.println((digitalRead(vab1EndstopTargetPin) == vab1EndstopEtatOn) ? "ON" : "OFF");

		Serial.print(F("Bouton SORTIE\t")); Serial.println((digitalRead(btn1EntreePin) == btn1EtatOn) ? "ON" : "OFF");
		Serial.print(F("Bouton ENTREE\t")); Serial.println((digitalRead(btn1SortiePin) == btn1EtatOn) ? "ON" : "OFF");

		Serial.print(F("Moteur ENABLE\t")); Serial.println((digitalRead(mpap1EnablePin) == LOW) ? "ON" : "OFF");

		delay(1000);
	}
}