#include <AccelStepper.h>

// Define el tipo de interfaz y los pines de control
#define PIN_STEP0 3
#define PIN_DIR0 2
#define PIN_STEP1 20
#define PIN_DIR1 21

AccelStepper stepper0(AccelStepper::DRIVER, PIN_STEP0, PIN_DIR0);
AccelStepper stepper1(AccelStepper::DRIVER, PIN_STEP1, PIN_DIR1);

// Configuración específica del motor
const float pasosPorGrado = 1.8; // Ajusta este valor a tu configuración

void setup() {
  Serial.begin(9600); // Inicia la comunicación serial a 9600 baudios
  
  // Configuración inicial para el primer stepper
  stepper0.setMaxSpeed(1000);
  stepper0.setAcceleration(200);
  
  // Configuración inicial para el segundo stepper
  stepper1.setMaxSpeed(1000);
  stepper1.setAcceleration(200);

  // Imprime la configuración del primer motor
  Serial.println("Configuracion del stepper0 (Cintura):");
  Serial.print("MAX SPEED: ");
  Serial.print(stepper0.maxSpeed());
  Serial.print(", ACCELERATION: ");
  Serial.print(stepper0.acceleration());
  Serial.print(", PIN STEP: ");
  Serial.print(PIN_STEP0);
  Serial.print(", PIN DIR: ");
  Serial.println(PIN_DIR0);
  
  // Imprime la configuración del segundo motor
  Serial.println("Configuracion del stepper1(Hombro):");
  Serial.print("MAX SPEED: ");
  Serial.print(stepper1.maxSpeed());
  Serial.print(", ACCELERATION: ");
  Serial.print(stepper1.acceleration());
  Serial.print(", PIN STEP: ");
  Serial.print(PIN_STEP1);
  Serial.print(", PIN DIR: ");
  Serial.println(PIN_DIR1);
}

void loop() {
  // Ejemplo de uso de la función
  // Mueve el stepper0 a 90 grados de posición absoluta
  moverMotorAAngulo(stepper0, 90, 'P');
  delay(2000); // Espera para observar el movimiento
  
  // Mueve el stepper1 45 grados más desde su posición actual
  moverMotorAAngulo(stepper1, 45, 'R');
  delay(2000); // Espera para observar el movimiento

  // Viendo el comportamiento de las funciones.
  moverMotorAAngulo(stepper0, 45, 'R');
  delay(2000); // Espera para observar el movimiento
}

void moverMotorAAngulo(AccelStepper &motor, float angulo, char tipoMovimiento) {
  long pasosAMover = angulo / pasosPorGrado; // Calcula los pasos a mover basado en el ángulo
  if (tipoMovimiento == 'P') { // Posición Absoluta
    motor.moveTo(pasosAMover); // Establece la posición objetivo absoluta
  } else if (tipoMovimiento == 'R') { // Posición Relativa
    motor.move(pasosAMover); // Establece la posición objetivo relativa
  }
  // Ejecuta el movimiento
  while (motor.distanceToGo() != 0) {
    motor.run();
  }
}