#include <ESP32Servo.h>  // Librería para el servo
// Pines del modulo C
#define IO1 8    // Sensor de no sujeción
#define IO2 7    // Sensor de sujeción
#define EN 12    
#define DIR 11   
#define STEP 10  
#define PWM 13  
// Servo
Servo separador;
int pinServo = PWM;  
int angulo;          // Ángulo actual del servo
// Variables
int contador = 0;        // total de pasos del recorrido
int pos_actual = 0;      // posición actual
int pos_sujecion = 0;
int pos_no_sujecion = 0;

void setup() {
  Serial.begin(115200);
  // Configuración de pines
  pinMode(IO1, INPUT);
  pinMode(IO2, INPUT);
  pinMode(EN, OUTPUT);
  pinMode(DIR, OUTPUT);
  pinMode(STEP, OUTPUT);

  digitalWrite(EN, LOW); // Habilita el driver (activo en bajo)
  // Configuración del servo
  separador.attach(pinServo, 500, 2400);
  angulo = 90;
  separador.write(angulo);
  Serial.println("Servo inicializado en 90° (posición horizontal)");
  // Calibración inicial
  Serial.println("Presione 'C' para iniciar calibración");
  while (Serial.read() != 'C');
  // Buscar el sensor de sujeción
  Serial.println("Buscando sensor de NO SUJECIÓN (IO1)...");
  digitalWrite(DIR, HIGH); // Mueve hacia el motor
  while (digitalRead(IO1) == LOW) {
    unPaso();
  }
  // Buscar el sensor de no sujeción (IO2)
  Serial.println("Buscando sensor de SUJECIÓN (IO2)...");
  digitalWrite(DIR, LOW); // Mueve hacia el lado lejano
  while (digitalRead(IO2) == LOW) {
    unPaso();
    contador++;
  }
  Serial.print("Longitud total del recorrido en pasos = ");
  Serial.println(contador);
  pos_no_sujecion = 0;
  pos_sujecion = contador;
  Serial.println("Calibración completa");
  Serial.println("Comandos disponibles:");
  Serial.println("1 -> Posición de SUJECIÓN (Motor hacia IO2, Servo 90°)");
  Serial.println("2 -> Posición de NO SUJECIÓN (Motor hacia IO1, Servo 0°)");
}

void loop() {
  if (Serial.available() > 0) {
    char comando = Serial.read();
    if (comando == '1') {
      // Posición 1: sujeción (lejano) + servo 90°
      Serial.println("Moviendo al sensor de sujeción");
      moverA(pos_sujecion);
      moverServo(90);
    }
    else if (comando == '2') {
      // Posición 2: no sujeción (cercano al motor) + servo 0°
      Serial.println("Moviendo al sensor de no sujeción");
      moverA(pos_no_sujecion);
      moverServo(0);
    }
  }
}
// Funciones
// Función: un paso del motor
void unPaso() {
  digitalWrite(STEP, HIGH);
  delayMicroseconds(1000); // Para variar la velocidad del motor a pasos variar el tiempo del pulso
  digitalWrite(STEP, LOW);
  delayMicroseconds(1000); // Para variar la velocidad del motor a pasos variar el tiempo del pulso
}
// Función: mover el motor a una posición dada
void moverA(int destino) {
  int pasos = abs(destino - pos_actual);
  if (pasos == 0) {
    Serial.println("Ya estás en esa posición.");
    return;
  }
  // Cambia la dirección dependiendo de la dirección a la que se debe mover (Hacia afuera/adentro)
  digitalWrite(DIR, (destino > pos_actual) ? LOW : HIGH);
  for (int i = 0; i < pasos; i++) {
    unPaso();
  }
  pos_actual = destino;
  Serial.print("Posición actual del motor = ");
  Serial.println(pos_actual);
}

// Función: mover el servo al nuevo ángulo
void moverServo(int nuevoAngulo) {
  if (nuevoAngulo < 0 || nuevoAngulo > 180) return; // rango del movimiento del servomotor CD
  int paso = (nuevoAngulo > angulo) ? 1 : -1;
  for (int i = angulo; i != nuevoAngulo; i += paso) { // Se mueve un grado/tiempo en delay
    separador.write(i);
    delay(1);
  }
  angulo = nuevoAngulo;
  Serial.printf("Servo movido a %d°\n", angulo);
}