/****************************************************************************************/
/*****          Tema:               Temporización                                   *****/
/*****          Proyecto:           Calibracion de un motor lineal                  *****/
/****************************************************************************************/
/*****          EvalBoard:          ESP32 S3 DevKitC C1                             *****/
/*****          Autor:              Rodia Santivañez Santivañez                     *****/
/*****          Fecha:              octubre 2024                                    *****/
/****************************************************************************************/
/***** Enunciado:                                                                   *****/
/****************************************************************************************/
/* Al inicio el motor no se encuentra calibrado. Verifique que los switchs esten en la  */
/* posicion abierta (derecha) antes de iniciar la calibracion.                          */
/* Utilice el pulsador central SEL del joystick para iniciar el proceso de calibracion. */
/* Al inciar la calibración el motor se desplaza hacia la derecha hasta cerrar el switch*/
/* derecho para luego invertir su desplazamiento hasta cerrar el switch izquierdo.      */
/* Cada desplazamiento tiene 5 segundos para completarse sino la calibración falla.     */
/* Como resultado de la calibración se muestra la carrera útil del motor en nro de pasos*/
/* Utilice el joystick de manera horizontal para mover el motor a voluntad (izq o der). */
/* El límite será entre 0 y la carrera calibrada. Si se mantiene el joystick en un      */
/* determinado sentido por más de 7seg la velocidad de desplazamiento aunmenta.         */
/****************************************************************************************/

#include <TM1637Display.h>

#define LED_PIN_R 6
#define LED_PIN_G 5
#define LED_PIN_B 4

#define DISPLAY__CLK      12
#define DISPLAY_DIO       11
#define JOYSTICK_HRZ      19
#define JOYSTICK_SEL      20

#define A4988_STEP    41			        // pin STEP de A4988 a pin 41
#define A4988_DIR     40			        // pin DIR de A4988 a pin 40
#define LIMIT_SWITCH_A  1             // limitador de carrera izquierdo
#define LIMIT_SWITCH_B  2             // limitador de carrera derecho

#define DERECHA       LOW
#define IZQUIERDA     HIGH
#define PRESIONADO    LOW
#define NO_PRESIONADO HIGH

unsigned long mmss_actuales = 0; //registra los milisegundos actuales
unsigned long mmss_iniciales = 0; //registra los milisegundos de referencia (iniciales)
bool isCalibrated = false; //indica si el motor está calibrado o no
bool isTimeout = false;    //vencimiento de tiempo para la calibración
uint16_t carrera = 0;      //carrera útil del motor en número de pasos
uint16_t posicion = 0;     //posición del motor lineal en numero de pasos
uint16_t persistencia;     //tiempo (ms) que en el joystick se persiste en un determnado sentido
uint16_t pulso;            //periodo de tiempo para el microstep

const uint8_t error[] = { SEG_G, SEG_G, SEG_G, SEG_G };

TM1637Display display(DISPLAY__CLK, DISPLAY_DIO); //creo mi objeto display

void LinearMotorMicrostep(uint8_t dir, uint8_t pulse) {
  digitalWrite(A4988_DIR, dir);           
  digitalWrite(A4988_STEP, HIGH);
  delay(pulse);
  digitalWrite(A4988_STEP, LOW);
  delay(pulse);
}

void OnCalibratedError() {
  display.setSegments(error);
  digitalWrite(LED_PIN_R, HIGH);
  digitalWrite(LED_PIN_G, LOW);
  Serial.println("Error de calibración: tiempo vencido");
  Serial.println("Calibre el motor lineal");
  isCalibrated = false;
}

void OnCalibratedSuccessful() {
  posicion = 0;
  display.showNumberDec(carrera);
  Serial.printf("Motor calibrado con carrera: %d\n\r", carrera);
  delay(2000);
  digitalWrite(LED_PIN_R, LOW);
  digitalWrite(LED_PIN_G, HIGH);
  display.showNumberDec(posicion);
  isCalibrated = true;
}

void setup() {
  display.setBrightness(0x0a);

  Serial.begin(115200);
  Serial.println("Hello, ESP32-S3!");

  pinMode(LED_PIN_R, OUTPUT); pinMode(LED_PIN_G, OUTPUT); pinMode(LED_PIN_B, OUTPUT); 
  pinMode(LIMIT_SWITCH_A, INPUT_PULLUP); 
  pinMode(LIMIT_SWITCH_B, INPUT_PULLUP);
  pinMode(JOYSTICK_HRZ, INPUT);
  pinMode(JOYSTICK_SEL, INPUT_PULLUP);
  pinMode(A4988_STEP, OUTPUT);	
  pinMode(A4988_DIR, OUTPUT);

  display.setSegments(error);
  digitalWrite(LED_PIN_R, HIGH);
  digitalWrite(LED_PIN_G, LOW);

  Serial.println("Sistemas Digitales B - lab08");
  Serial.println("Calibre el motor lineal");
}

void loop() {
  if(digitalRead(JOYSTICK_SEL) == LOW) {
    delay(10);
    while(digitalRead(JOYSTICK_SEL) == LOW);
    delay(10);

    digitalWrite(LED_PIN_R, HIGH);
    digitalWrite(LED_PIN_G, HIGH);
    display.setSegments(error);
    Serial.println("Inicio de calibracion");
    carrera = 0;
    mmss_iniciales = millis();
    isTimeout = false;

    while(digitalRead(LIMIT_SWITCH_B) == HIGH && !isTimeout) { // 2 condiciones: el switch está abierto
      LinearMotorMicrostep(DERECHA, 20);                       // y el tiempo no se ha vencido
      if((millis() - mmss_iniciales) > 5000) isTimeout = true;
    }
    if(isTimeout) { // si el tiempo se vence la calibración falla
      OnCalibratedError();      
    }
    else {   // se invierte el giro del motor         
      mmss_iniciales = millis();
      isTimeout = false;

      while(digitalRead(LIMIT_SWITCH_A) == HIGH && !isTimeout) { // 2 condiciones: el switch está abierto
        LinearMotorMicrostep(IZQUIERDA, 20);                     // y el tiempo no se ha vencido      
        if((millis() - mmss_iniciales) > 5000) isTimeout = true;
        carrera++;
      }
      if(isTimeout) {  // si el tiempo se vence la calibración falla
        OnCalibratedError();
      }
      else {    //el motor se considera calibrado
        OnCalibratedSuccessful();        
      }      
    }
  }
  if(isCalibrated) {
    if(analogRead(JOYSTICK_HRZ) > 2048) {
      mmss_iniciales = millis();
      while(analogRead(JOYSTICK_HRZ) > 2048) {
        if(posicion > 0) {         
          persistencia = millis() - mmss_iniciales;          
          if(persistencia < 5000) pulso = 250; //persiste < 5seg -> velocidad baja
          else pulso = 10;                     //persiste > 5seg -> velocidad alta
          LinearMotorMicrostep(IZQUIERDA, pulso);          
          display.showNumberDec(--posicion);
        }
      }          
    }
    else if(analogRead(JOYSTICK_HRZ) < 2048) {   
      mmss_iniciales = millis();   
      while(analogRead(JOYSTICK_HRZ) < 2048) {
        if(posicion < carrera) {
          persistencia = millis() - mmss_iniciales;          
          if(persistencia < 5000) pulso = 250; //persiste < 5seg -> velocidad baja
          else pulso = 10;                     //persiste > 5seg -> velocidad alta
          LinearMotorMicrostep(DERECHA, pulso);          
          display.showNumberDec(++posicion);
        }
      }       
    }
  }  
}