#include <AccelStepper.h>

// Definición de pines para el motor X
#define X_STEP_PIN 5
#define X_DIR_PIN 4

// Definición de pines para el motor Y
#define Y_STEP_PIN 7
#define Y_DIR_PIN 6

// Número total de pulsos
#define TOTAL_PULSES 10000

// Crear instancias de AccelStepper para los motores
AccelStepper stepperX(AccelStepper::FULL4WIRE, X_STEP_PIN, X_DIR_PIN);
AccelStepper stepperY(AccelStepper::FULL4WIRE, Y_STEP_PIN, Y_DIR_PIN);

void setup() {
  // Inicializar comunicación serial
  Serial.begin(9600);

  // Configurar los pines de los motores como salidas
  pinMode(X_STEP_PIN, OUTPUT);
  pinMode(X_DIR_PIN, OUTPUT);
  pinMode(Y_STEP_PIN, OUTPUT);
  pinMode(Y_DIR_PIN, OUTPUT);

  // Configurar la velocidad máxima y la aceleración de los motores
  stepperX.setMaxSpeed(100);  // Máxima velocidad para el motor X
  stepperY.setMaxSpeed(100);  // Máxima velocidad para el motor Y
}

void loop() {
  // Mover el motor X hacia adelante
  for (int i = 0; i < TOTAL_PULSES; ++i) {
    stepperX.setSpeed(100);  // Establecer la velocidad
    stepperX.runSpeed();    // Ejecutar el motor
    delay(1);

    // Enviar posición a la consola cada 10 pulsos
    if (i % 10 == 0) {
      Serial.print("X: ");
      Serial.println(stepperX.currentPosition());
    }
  }

  // Mover el motor X hacia atrás
  for (int i = TOTAL_PULSES; i > 0; --i) {
    stepperX.setSpeed(-100);  // Establecer la velocidad en reversa
    stepperX.runSpeed();     // Ejecutar el motor
    delay(1);

    // Enviar posición a la consola cada 10 pulsos
    if (i % 10 == 0) {
      Serial.print("X: ");
      Serial.println(stepperX.currentPosition());
    }
  }

  // Mover el motor Y hacia adelante
  for (int i = 0; i < TOTAL_PULSES; ++i) {
    stepperY.setSpeed(100);  // Establecer la velocidad
    stepperY.runSpeed();    // Ejecutar el motor
    delay(1);

    // Enviar posición a la consola cada 10 pulsos
    if (i % 10 == 0) {
      Serial.print("Y: ");
      Serial.println(stepperY.currentPosition());
    }
  }

  // Mover el motor Y hacia atrás
  for (int i = TOTAL_PULSES; i > 0; --i) {
    stepperY.setSpeed(-100);  // Establecer la velocidad en reversa
    stepperY.runSpeed();     // Ejecutar el motor
    delay(1);

    // Enviar posición a la consola cada 10 pulsos
    if (i % 10 == 0) {
      Serial.print("Y: ");
      Serial.println(stepperY.currentPosition());
    }
  }

  // Reiniciar posición de los motores a 0
  stepperX.setCurrentPosition(0);
  stepperY.setCurrentPosition(0);
}