// =====================================================
// FRESADORA AUTOMATICA XYZ
// VERSION MEJORADA
// PWM MANUAL SIN LIBRERIA SERVO
// =====================================================

// =====================================================
// A4988
// =====================================================

#define A4988_EN    12
#define A4988_DIR   11
#define A4988_STEP  10

// =====================================================
// FINALES DE CARRERA
// =====================================================

#define SW_SUJECION      7
#define SW_NO_SUJECION   8

// =====================================================
// DIRECCIONES
// =====================================================

#define DIR_NO_SUJECION  HIGH
#define DIR_SUJECION     LOW

// =====================================================
// SERVOMOTOR
// =====================================================

const uint8_t servoPin = 5;

const uint8_t PWM_FREQ = 50;
const uint8_t PWM_RES  = 14;

const uint16_t PWM_MAX = (1 << PWM_RES) - 1;

// =====================================================
// SERIAL
// =====================================================

const uint8_t max_elementos = 20;

char comando[max_elementos];

int cont = 0;

// =====================================================
// VARIABLE DE MODOS
// =====================================================

int modo = 0;

// =====================================================
// SETUP
// =====================================================

void setup() {

  Serial.begin(115200);

  // ---------------------------------------------------
  // CONFIGURACION PWM
  // ---------------------------------------------------

  ledcAttach(servoPin, PWM_FREQ, PWM_RES);

  // ---------------------------------------------------
  // CONFIGURACION A4988
  // ---------------------------------------------------

  pinMode(A4988_EN, OUTPUT);

  pinMode(A4988_DIR, OUTPUT);

  pinMode(A4988_STEP, OUTPUT);

  // ---------------------------------------------------
  // FINALES DE CARRERA
  // ---------------------------------------------------

  pinMode(SW_SUJECION, INPUT_PULLUP);

  pinMode(SW_NO_SUJECION, INPUT_PULLUP);

  // ---------------------------------------------------
  // HABILITAR DRIVER
  // ---------------------------------------------------

  digitalWrite(A4988_EN, LOW);

  // ---------------------------------------------------
  // MENSAJE
  // ---------------------------------------------------

  Serial.println("=================================");
  Serial.println("FRESADORA AUTOMATICA XYZ");
  Serial.println("=================================");

  // ---------------------------------------------------
  // CALIBRACION
  // ---------------------------------------------------

  CalibracionInicial();

  // ---------------------------------------------------
  // MENU
  // ---------------------------------------------------

  Serial.println("Ingrese modo:");
  Serial.println("M1 -> TEST");
  Serial.println("M2 -> REGISTRO");
  Serial.println("M3 -> OPERACION");
}

// =====================================================
// LOOP
// =====================================================

void loop() {

  // ---------------------------------------------------
  // VERIFICAR DATOS SERIAL
  // ---------------------------------------------------

  if (Serial.available()) {

    // Leer caracter

    char c = Serial.read();

    // -------------------------------------------------
    // ENTER
    // -------------------------------------------------

    if (c == '\n') {

      // Finalizar string

      comando[cont] = '\0';

      // Procesar comando

      procesarComando(comando);

      // Reiniciar contador

      cont = 0;
    }

    // -------------------------------------------------
    // GUARDAR CARACTERES
    // -------------------------------------------------

    else {

      if (cont < max_elementos - 1) {

        comando[cont++] = c;
      }

      else {

        Serial.println("Error: comando demasiado largo");

        cont = 0;

        reiniciar_buffer();
      }
    }
  }
}

// =====================================================
// PROCESAR COMANDOS
// =====================================================

void procesarComando(char *cmd) {

  // ---------------------------------------------------
  // MODOS
  // ---------------------------------------------------

  if (strcmp(cmd, "M1") == 0) {

    modo = 1;

    Serial.println("Modo TEST activado");

    return;
  }

  if (strcmp(cmd, "M2") == 0) {

    modo = 2;

    Serial.println("Modo REGISTRO activado");

    return;
  }

  if (strcmp(cmd, "M3") == 0) {

    modo = 3;

    Serial.println("Modo OPERACION activado");

    return;
  }

  // ---------------------------------------------------
  // ESC
  // ---------------------------------------------------

  if (strcmp(cmd, "ESC") == 0) {

    modo = 0;

    Serial.println("Saliendo del modo");

    return;
  }

  // ---------------------------------------------------
  // MODO TEST
  // ---------------------------------------------------

  if (modo == 1) {

    estados(cmd);
  }

  // ---------------------------------------------------
  // MODO REGISTRO
  // ---------------------------------------------------

  else if (modo == 2) {

    Serial.print("Estado registrado: ");

    Serial.println(cmd);
  }

  // ---------------------------------------------------
  // MODO OPERACION
  // ---------------------------------------------------

  else if (modo == 3) {

    estados(cmd);
  }

  else {

    Serial.println("Seleccione un modo primero");
  }
}

// =====================================================
// FUNCION DE ESTADOS
// =====================================================

void estados(char *cmd) {

  // ---------------------------------------------------
  // A0
  // ---------------------------------------------------

  if (strcmp(cmd, "A0") == 0) {

    Serial.println("Plataforma A -> PNS");

    digitalWrite(A4988_DIR, DIR_NO_SUJECION);

    MoverMotor('A');

    return;
  }

  // ---------------------------------------------------
  // A1
  // ---------------------------------------------------

  if (strcmp(cmd, "A1") == 0) {

    Serial.println("Plataforma A -> PS");

    digitalWrite(A4988_DIR, DIR_SUJECION);

    MoverMotor('A');

    return;
  }

  // ---------------------------------------------------
  // B0
  // ---------------------------------------------------

  if (strcmp(cmd, "B0") == 0) {

    Serial.println("Plataforma B -> PNS");

    digitalWrite(A4988_DIR, DIR_NO_SUJECION);

    MoverMotor('B');

    return;
  }

  // ---------------------------------------------------
  // B1
  // ---------------------------------------------------

  if (strcmp(cmd, "B1") == 0) {

    Serial.println("Plataforma B -> PS");

    digitalWrite(A4988_DIR, DIR_SUJECION);

    MoverMotor('B');

    return;
  }

  // ---------------------------------------------------
  // Z0
  // ---------------------------------------------------

  if (strcmp(cmd, "Z0") == 0) {

    Serial.println("Servo -> 90 grados");

    writeServoAngle(90);

    return;
  }

  // ---------------------------------------------------
  // Z1
  // ---------------------------------------------------

  if (strcmp(cmd, "Z1") == 0) {

    Serial.println("Servo -> 140 grados");

    writeServoAngle(140);

    return;
  }

  // ---------------------------------------------------
  // ERROR
  // ---------------------------------------------------

  Serial.println("Comando invalido");
}

// =====================================================
// MOVER MOTOR PASO A PASO
// =====================================================

void MoverMotor(char motor) {

  int pasos = 0;

  // ---------------------------------------------------
  // MOTOR A
  // ---------------------------------------------------

  if (motor == 'A') {

    pasos = 280;
  }

  // ---------------------------------------------------
  // MOTOR B
  // ---------------------------------------------------

  else if (motor == 'B') {

    pasos = 390;
  }

  // ---------------------------------------------------
  // GENERAR PASOS
  // ---------------------------------------------------

  for (int i = 0; i < pasos; i++) {

    digitalWrite(A4988_STEP, HIGH);

    delay(2);

    digitalWrite(A4988_STEP, LOW);

    delay(2);
  }
}

// =====================================================
// CALIBRACION INICIAL
// =====================================================

void CalibracionInicial() {

  Serial.println("Iniciando calibracion...");

  digitalWrite(A4988_DIR, DIR_NO_SUJECION);

  // Mover hasta final de carrera

  while (digitalRead(SW_NO_SUJECION) == LOW) {

    digitalWrite(A4988_STEP, HIGH);

    delay(2);

    digitalWrite(A4988_STEP, LOW);

    delay(2);
  }

  // Servo inicial

  writeServoAngle(90);

  Serial.println("Calibracion completada");
}

// =====================================================
// PWM MANUAL PARA SERVO
// =====================================================

void writeServoAngle(uint8_t angle) {

  float dutyPercent = 2.5 + (angle * (10.0 / 180.0));

  uint32_t duty = (uint32_t)((dutyPercent / 100.0) * PWM_MAX);

  ledcWrite(servoPin, duty);
}

// =====================================================
// LIMPIAR BUFFER SERIAL
// =====================================================

void reiniciar_buffer() {

  while (Serial.available()) {

    Serial.read();
  }
}