//==================================================================================================================================================================================
// Autor...........: Cid Rodrigo Barcellos Filho
// Cliente.........: Laura Lima (Artista Plástica), Yoann Saura (Francês Art&Tech)
// Projeto.........: Obra-5_Bate-Bola - Arduino-1 (de Cima)
// Objetivo........: Movimentar 3 Motor de Passo sincronizados para animação da Obra-5_Bate-Bola
//					 O Eixo desses Motores estão conectados a carreteis para subir e descer a barra superior da Obra-5.
//					 Posição dos Motores na Obra: Motor-X -> "do Rabo" , Motor-Y -> "do Meio" e Motor-Z -> "da Cabeça".
// Arquivo.........: Ob5-Cima_V03.ino
// Data............: 09/03/2023
// ConfigArduinoIDE: Versão_Ide: 1.8.13 , Placa: "Arduino Uno"
// Hardware........: 1 Arduino Uno , 1 CNC Shield V3.0 com 3 Drivers, 3 Motores de Passo Nema-17,
//                   3 Sensores de Fim de Curso (à definir),
//==================================================================================================================================================================================

//============================================================================================================================================
#define MODO_DEBUG //Deixar essa linha comentada para NÃO Debugar pela Serial.
//============================================================================================================================================

//=================================================================================================================================================================
// Bibliotecas Utilizadas e Declaração/Definição de Variáveis/Constantes Globais
//=================================================================================================================================================================

#define DELAY_INICIAL			30000	//Delay Inicial em mili-segundos antes de começar os movimentos [no setup()]
#define DELAY_ENTRE_MOVIMENTOS	1		//Delay em mili-segundos entre os movimentos.
#define QTD_PASSOS_POR_VOLTA    200		//Constante para o Nema-17 utilizado (Full-Step)
#define UMA_ITERACAO_FOR_US		90		//Tempo de uma Iteração do FOR em micro-segundos.
#define _H_						true	//Direção Horária dos Ponteiros do Relógio (Visão de cima do Motor)
#define _A_						false	//Direção Anti-Horária dos Ponteiros do Relógio (Visão de cima do Motor)
#define _P_						0		//Motor Parado (Alimentado = Travado)

const int Pin_Enable = 8;		//Pinos de controle dos 3 Motores

const int Pin_Dir_X  = 5; 		//Pinos de controle do Motor-X 
const int Pin_Step_X = 2 ;

const int Pin_Dir_Y  = 6; 		//Pinos de controle do Motor-Y
const int Pin_Step_Y = 3;

const int Pin_Dir_Z  = 7; 		//Pinos de controle do Motor-Z
const int Pin_Step_Z = 4;

unsigned long G_millis = 0;		//Variável Global para Receber o millis()



//=================================================================================================================================================================
// Definição de Funções Gerais
//=================================================================================================================================================================

//----------------------------------------------------------------------------------------------------------------------------------
//  Função de ajuda para Obter o Maior Número entre três valores
//----------------------------------------------------------------------------------------------------------------------------------
long fnc_GetMaiorNum(long num1, long num2, long num3) {
	long Maior = num1;	//Inicialmente considero que o maior é o num1

	if (num2 > Maior) Maior = num2;	//Salvo o maior número caso num2 seja maior
	if (num3 > Maior) Maior = num3;	//Salvo o maior número caso num3 seja maior

	return Maior;	//Retorna o maior número
}
//----------------------------------------------------------------------------------------------------------------------------------


//----------------------------------------------------------------------------------------------------------------------------------
// Função principal para Ligar os Motores. Cada chamada executa um "Movimento do conjunto" para os 3 motores em um determinado tempo.
//----------------------------------------------------------------------------------------------------------------------------------
void fnc_Liga_Motor(String Nome_Movimento, long Periodo_seg, bool Direcao_X, bool Direcao_Y, bool Direcao_Z, long Num_Voltas_X, long Num_Voltas_Y, long Num_Voltas_Z){

	//--Cálculos Necessários apartir dos parâmetros Recebidos------------------------------------------------------------
	long Maior_Num_Voltas = fnc_GetMaiorNum(Num_Voltas_X, Num_Voltas_Y, Num_Voltas_Z);

    long Num_Passos 	= Maior_Num_Voltas * QTD_PASSOS_POR_VOLTA;		//Maior Número de Passos entre os três 
    long Delay_us_For   = round( ( ( ( (float)(Periodo_seg * 1000) / Num_Passos) * 1000) - UMA_ITERACAO_FOR_US ) / 2); 	//Delay para ajustar o Time dentro do For

	long Num_Passos_x12 = Num_Passos * 12;		//Variáveis utilizadas no FOR ao invés da Num_Passos e Delay_us_For
	long Delay_us_For_d6 = Delay_us_For / 6;

	long ChangeLevel_X = 0, ChangeLevel_Y = 0, ChangeLevel_Z = 0;		
	long Num_Voltas_REAL_X = 0, Num_Voltas_REAL_Y = 0, Num_Voltas_REAL_Z = 0;	//

	if (Num_Voltas_X == 0) {
		ChangeLevel_X = -1;	//-1 Nunca será alcançado no loop do For => Motor Parado
	} else {
		ChangeLevel_X = round(((float)Maior_Num_Voltas / Num_Voltas_X) * 6);
		Num_Voltas_REAL_X = (6.0F / ChangeLevel_X) * Maior_Num_Voltas;
	}

	if (Num_Voltas_Y == 0) {
		ChangeLevel_Y = -1;	//-1 Nunca será alcançado no loop do For => Motor Parado
	} else {
		ChangeLevel_Y = round(((float)Maior_Num_Voltas / Num_Voltas_Y) * 6);
		Num_Voltas_REAL_Y = (6.0F / ChangeLevel_Y) * Maior_Num_Voltas;
	}

	if (Num_Voltas_Z == 0) {
		ChangeLevel_Z = -1;	//-1 Nunca será alcançado no loop do For => Motor Parado
	} else {
		ChangeLevel_Z = round(((float)Maior_Num_Voltas / Num_Voltas_Z) * 6);
		Num_Voltas_REAL_Z = (6.0F / ChangeLevel_Z) * Maior_Num_Voltas;
	}
	//----------------------------------------------------------------------------------------------------------------------

	//--Variáveis para manter o Nível Lógico do Pino Step de cada motor (o toggle é feito dentro do FOR)
	bool F_Nivel_X = false, F_Nivel_Y = false, F_Nivel_Z = false;

	//--Variáveis para contagens dentro do FOR.
	long F_iX = 0, F_iY = 0, F_iZ = 0;

	long T_X = 0, T_Y = 0, T_Z = 0;

	#ifdef MODO_DEBUG
		Serial.println("-------------------------------------------------------");
		Serial.println("---> " + Nome_Movimento);
		Serial.println("-------------------------------------------------------");
		Serial.println("Periodo_seg = " + String(Periodo_seg));
		Serial.println("Direcao_X = " + String(Direcao_X) + " , Num_Voltas_X = " + String(Num_Voltas_X) + " , ChangeLevel_X = " + String(ChangeLevel_X) + " , Num_Voltas_REAL_X = " + String(Num_Voltas_REAL_X));
		Serial.println("Direcao_Y = " + String(Direcao_Y) + " , Num_Voltas_Y = " + String(Num_Voltas_Y) + " , ChangeLevel_Y = " + String(ChangeLevel_Y) + " , Num_Voltas_REAL_Y = " + String(Num_Voltas_REAL_Y));
		Serial.println("Direcao_Z = " + String(Direcao_Z) + " , Num_Voltas_Z = " + String(Num_Voltas_Z) + " , ChangeLevel_Z = " + String(ChangeLevel_Z) + " , Num_Voltas_REAL_Z = " + String(Num_Voltas_REAL_Z));
		Serial.println("Num_Passos = " + String(Num_Passos));
		Serial.println("Delay_us_For = " + String(Delay_us_For));
		Serial.println("Num_Passos_x12 = " + String(Num_Passos_x12) + " -> (Igual as Iterações do For)");
		Serial.println("Delay_us_For_d6 = " + String(Delay_us_For_d6) + " -> (Delay Adicionado a cada Iteração do For)");
		if (Delay_us_For_d6 < 60) {
			Serial.println(">>> ATENÇÃO: VALOR MUITO BAIXO (<100uS) PARA NO DELAY ADICIONADO <<<");
		}
		Serial.println("Movimento Iniciado. Aguarde...");
	#endif

    digitalWrite(Pin_Dir_X, Direcao_X); 
    digitalWrite(Pin_Dir_Y, Direcao_Y); 
    digitalWrite(Pin_Dir_Z, Direcao_Z); 

	G_millis = millis();	//Início da Cronometragem do tempo de Execução do FOR

	//----------------------------------------------------------------
	// FOR a execução do movimento dos 3 motores com um tempo previsto de [Periodo_seg],
	// Baseado no tempo de execução do Arduino Uno à 16MHz.
	//----------------------------------------------------------------
    for(long i = 0; i < Num_Passos_x12; i++) {

		if (F_iX == 0) {
			F_Nivel_X = !F_Nivel_X;		
			digitalWrite(Pin_Step_X, F_Nivel_X);	//Inverte o Nivel Lógico do Pin_Step_X
			T_X++;
		}
		if (F_iY == 0) {
			F_Nivel_Y = !F_Nivel_Y; 
			digitalWrite(Pin_Step_Y, F_Nivel_Y);	//Inverte o Nivel Lógico do Pin_Step_Y
			T_Y++;
		}
		if (F_iZ == 0) {
			F_Nivel_Z = !F_Nivel_Z; 
			digitalWrite(Pin_Step_Z, F_Nivel_Z);	//Inverte o Nivel Lógico do Pin_Step_Z
			T_Z++;
		}

		F_iX++; F_iY++; F_iZ++;						//Incrementa os 3 Contadores e Zera ao Alcançar o seu Limite em ChangeLevel_
		if (F_iX == ChangeLevel_X) F_iX = 0;		
		if (F_iY == ChangeLevel_Y) F_iY = 0;
		if (F_iZ == ChangeLevel_Z) F_iZ = 0;

		delayMicroseconds(Delay_us_For_d6); 	//Delay Adicionado a cada Iteração do FOR

    }
	//----------------------------------------------------------------

	unsigned long Real_ms = millis() - G_millis;  //Final da Cronometragem do tempo de Execução do FOR

	#ifdef MODO_DEBUG
		Serial.println("Periodo Real Cronometrado : " + String( float(Real_ms) / 1000 , 4 ) + " segundos.");
		Serial.println("T_X = " + String(T_X) + " , T_Y = " + String(T_Y) + " , T_Z = " + String(T_Z));
		Serial.println("VoltasContadas: X = " + String(T_X/400.0) + " , Y = " + String(T_Y/400.0) + " , Z = " + String(T_Z/400.0));
		Serial.println("PassosContados: X = " + String(T_X/2) + " , Y = " + String(T_Y/2) + " , Z = " + String(T_Z/2));
		Serial.println("");
	#endif

}
//----------------------------------------------------------------------------------------------------------------------------------

//=================================================================================================================================================================



//=================================================================================================================================================================
// Função SETUP
//=================================================================================================================================================================
void setup ( ) {  

	#ifdef MODO_DEBUG
		Serial.begin(115200);
	#endif

    pinMode (Pin_Step_X, OUTPUT );
    pinMode (Pin_Dir_X,  OUTPUT );

    pinMode (Pin_Step_Y, OUTPUT );
    pinMode (Pin_Dir_Y,  OUTPUT );

    pinMode (Pin_Step_Z, OUTPUT );
    pinMode (Pin_Dir_Z,  OUTPUT );

    pinMode (Pin_Enable, OUTPUT );
    digitalWrite(Pin_Enable, LOW); 	//Habilita todos os Motores

	#ifdef MODO_DEBUG
		Serial.println("#############################################################");
		Serial.println("# Obra-5_Bate-Bola - Arduino-1 (de Cima) # Ob5-Cima_V03.ino #");
		Serial.println("#############################################################\n");
		Serial.println("Aguarde o Delay Inicial no Setup = " + String(DELAY_INICIAL) + " ms.\n");
	#endif

	delay(DELAY_INICIAL);
}
//=================================================================================================================================================================


//=================================================================================================================================================================
// Função LOOP
//=================================================================================================================================================================
void loop() {

	//Parâmetros da fnc_Liga_Motor: (Nome_Movimento, Periodo_seg, Direcao_X, Direcao_Y, Direcao_Z, Num_Voltas_X, Num_Voltas_Y, Num_Voltas_Z)

	//Enquanto não tem sensor de fim de Curso, Será necessário Posicionar Manualmente todos os carreteis para o Início da apresentação, ou seja com a obra fechada.

    fnc_Liga_Motor("Movimento 1", 90, _A_,_A_,_A_ , 274, 149, 274);		//Movimento 1
    delay(DELAY_ENTRE_MOVIMENTOS);
    
	  fnc_Liga_Motor("Movimento 2", 90, _H_,_A_,_H_ , 125, 125, 125);		//Movimento 2
    delay(DELAY_ENTRE_MOVIMENTOS);

    fnc_Liga_Motor("Movimento 3", 90, _A_,_H_,_P_ , 125, 62, 0);		//Movimento 3
    delay(DELAY_ENTRE_MOVIMENTOS);

    fnc_Liga_Motor("Movimento 4", 90, _H_,_P_,_A_ , 125, 0, 125);		//Movimento 4
    delay(DELAY_ENTRE_MOVIMENTOS);

    fnc_Liga_Motor("Movimento 5", 90, _P_,_A_,_H_ , 0, 62, 125);		//Movimento 5
    delay(DELAY_ENTRE_MOVIMENTOS);

	//Considerando Sentido AntiHorário como (-) e Sentido Horário como (+),
	//após todos os 5 Movimentos, teremos o seguinte saldo de Voltas com valores AntiHorário: 
	//Motor-X ("Rabo")..: -274 +125 -125 +125   +0 = -149
	//Motor-Y ("Meio")..: -149 -125  +62   +0  -62 = -274
	//Motor-Z ("Cabeça"): -274 +125   +0 -125 +125 = -149
	//Enquanto não tem sensor de fim de Curso, Segue abaixo duas chamadas da fnc_Liga_Motor para Zerar as Posições, ou seja,
	//voltar para o Ínicio com a obra fechada para permitir o Loop.

    fnc_Liga_Motor("Movimento Retorno 1/2", 45, _P_,_H_,_P_ , 0, 125, 0);		//Movimento de Retorno
    fnc_Liga_Motor("Movimento Retorno 2/2", 45, _H_,_H_,_H_ , 149, 149, 149);
    delay(DELAY_ENTRE_MOVIMENTOS);

}
//==================================================================================================================================