#include <AccelStepper.h>

// Define los pines de STEP, DIR y ENABLE del driver A4988 
// (MOTOR 1)
#define DIR_PIN1 2
#define STEP_PIN1 4
#define ENABLE_PIN1 5 // Puede estar conectado directamente a GND si se quiere mantener habilitado siempre
// (MOTOR 2)
#define DIR_PIN2 12
#define STEP_PIN2 13
#define ENABLE_PIN2 14 // Puede estar conectado directamente a GND si se quiere mantener habilitado siempre
const int velocidadMotor = 3000;
const int acceleracionMotor = 1000;

//Movimiento en Pasos
//(MOTOR 1)
const int pasosAvanceMotor1 = 400;
const int pasosRetrocesoMotor1 = -200;

//(MOTOR 2)
const int pasosAvanceMotor2 = 600;
const int pasosRetrocesoMotor2 = -200;

// Tiempo de espera entre movimientos
const int tiempoDeEspera = 100;


// Crea una instancia de AccelStepper
// (MOTOR 1)
AccelStepper stepper1(AccelStepper::DRIVER, STEP_PIN1, DIR_PIN1);
// (MOTOR 2)
AccelStepper stepper2(AccelStepper::DRIVER, STEP_PIN2, DIR_PIN2);

void setup() {
  // Establece la velocidad máxima y la aceleración del motor (puedes ajustar estos valores según tus necesidades)
  
  //(MOTOR 1)
  stepper1.setMaxSpeed(velocidadMotor); // Velocidad máxima en pasos por segundo
  stepper1.setAcceleration(acceleracionMotor); // Aceleración en pasos por segundo al cuadrado
   //(MOTOR 2)
  stepper2.setMaxSpeed(velocidadMotor); // Velocidad máxima en pasos por segundo
  stepper2.setAcceleration(acceleracionMotor); // Aceleración en pasos por segundo al cuadrado



  // Configura el pin ENABLE como salida

  //(MOTOR 1)
  pinMode(ENABLE_PIN1, OUTPUT);
  digitalWrite(ENABLE_PIN1, LOW); // Habilita el driver (ajusta a HIGH si lo conectaste a VCC en lugar de GND)
  //(MOTOR 2)
  pinMode(ENABLE_PIN2, OUTPUT);
  digitalWrite(ENABLE_PIN2, LOW); // Habilita el driver (ajusta a HIGH si lo conectaste a VCC en lugar de GND)


}

void loop() {
  // Gira el motor en un sentido durante 2 segundos
  
  //(MOTOR 1)
  // Avance
  stepper1.setSpeed(velocidadMotor); // Velocidad en pasos por segundo (puedes ajustarla)
  stepper1.move(pasosAvanceMotor1); // Número de pasos a mover
  stepper1.runToPosition(); // Espera hasta que termine el movimiento

  delay(tiempoDeEspera); // Espera 1 segundo

  // Gira el motor en el sentido opuesto durante 2 segundos
  //Retroceso
  stepper1.setSpeed(velocidadMotor*-1); // Velocidad en pasos por segundo (puedes ajustarla)
  stepper1.move(pasosRetrocesoMotor1); // Número de pasos a mover (usamos un valor negativo para el sentido opuesto)
  stepper1.runToPosition(); // Espera hasta que termine el movimiento

  delay(tiempoDeEspera); // Espera 1 segundo

  //(MOTOR 2)
  // Avance
  stepper2.setSpeed(velocidadMotor); // Velocidad en pasos por segundo (puedes ajustarla)
  stepper2.move(pasosAvanceMotor2); // Número de pasos a mover
  stepper2.runToPosition(); // Espera hasta que termine el movimiento

  delay(tiempoDeEspera); // Espera 1 segundo

  // Gira el motor en el sentido opuesto durante 2 segundos
  //Retroceso
  stepper2.setSpeed(velocidadMotor*-1); // Velocidad en pasos por segundo (puedes ajustarla)
  stepper2.move(pasosRetrocesoMotor2); // Número de pasos a mover (usamos un valor negativo para el sentido opuesto)
  stepper2.runToPosition(); // Espera hasta que termine el movimiento

  delay(tiempoDeEspera); // Espera 1 segundo

}


/*

const int DIR = 2;
const int STEP = 4;
const int  steps_per_rev = 100;
const int timepoPaso = 1000;

void setup()
{
  Serial.begin(115200);
  pinMode(STEP, OUTPUT);
  pinMode(DIR, OUTPUT);
}
void loop()
{
  digitalWrite(DIR, HIGH);
  Serial.println("Girando en sentido horario");
  for(int j = 0; j<=3; j++)
  {
     for(int i = 0; i<steps_per_rev; i++)
     {
      digitalWrite(STEP, HIGH);
      delayMicroseconds(timepoPaso);
      digitalWrite(STEP, LOW);
      delayMicroseconds(timepoPaso);
     }
  }
  delay(1000); 
  
  digitalWrite(DIR, LOW);
  Serial.println("Girando en sentido anti horario");

  for(int i = 0; i<steps_per_rev; i++)
  {
    digitalWrite(STEP, HIGH);
    delayMicroseconds(timepoPaso);
    digitalWrite(STEP, LOW);
    delayMicroseconds(timepoPaso);
  }
  delay(1000);
}
*/