#include <ESP32Servo.h> // Librería específica para ESP32

// --- DEFINICIÓN DE LOS 6 SERVOS DEL BRAZO ---
Servo base;       // SERVO 1
Servo hombro;     // SERVO 2
Servo codo;       // SERVO 3
Servo munecaRots; // SERVO 4
Servo munecaFlex; // SERVO 5
Servo pinza;      // SERVO 6

// Pines recomendados para ESP32 (GPIOs libres de funciones especiales)
const int pinServo1 = 13; // SERVO 1
const int pinServo2 = 12; // SERVO 2
const int pinServo3 = 14; // SERVO 3
const int pinServo4 = 27; // SERVO 4
const int pinServo5 = 26; // SERVO 5
const int pinServo6 = 25; // SERVO 6

void setup() {
  Serial.begin(115200); // ESP32 suele usar 115200 baudios
  
  // En ESP32 es necesario configurar el temporizador antes de hacer el attach
  ESP32PWM::allocateTimer(0);
  ESP32PWM::allocateTimer(1);
  ESP32PWM::allocateTimer(2);
  ESP32PWM::allocateTimer(3);

  // --- ASIGNACIÓN DE PINES (Configuración MG995) ---
  // Los límites 500us y 2400us se mantienen igual
  base.setPeriodHertz(50);       // Frecuencia estándar de 50Hz para servos
  base.attach(pinServo1, 500, 2400); 
  
  hombro.setPeriodHertz(50);
  hombro.attach(pinServo2, 500, 2400);
  
  codo.setPeriodHertz(50);
  codo.attach(pinServo3, 500, 2400);
  
  munecaRots.setPeriodHertz(50);
  munecaRots.attach(pinServo4, 500, 2400);
  
  munecaFlex.setPeriodHertz(50);
  munecaFlex.attach(pinServo5, 500, 2400);
  
  pinza.setPeriodHertz(50);
  pinza.attach(pinServo6, 500, 2400);

  // --- POSICIÓN INICIAL DE REPOSO (HOME) ---
  base.write(90);       
  hombro.write(90);     
  codo.write(90);       
  munecaRots.write(90); 
  munecaFlex.write(90); 
  pinza.write(90);      
  
  delay(2000); 
  Serial.println("Secuencia ESP32 con 6 servos iniciada...");
}

void loop() {
  // --- PASO 1: Abrir pinza (SERVO 6) y girar base (SERVO 1) ---
  Serial.println("Abriendo pinza y girando base...");
  moverSuave(pinza, 90, 150, 20); // SERVO 6
  moverSuave(base, 90, 45, 30);   // SERVO 1
  delay(500);

  // --- PASO 2: Bajar el brazo (SERVO 2 y SERVO 3) ---
  Serial.println("Bajando brazo...");
  moverSuave(hombro, 90, 130, 40); // SERVO 2
  moverSuave(codo, 90, 60, 40);    // SERVO 3
  delay(500);

  // --- PASO 3: Cerrar pinza (SERVO 6) ---
  Serial.println("Sujetando objeto...");
  moverSuave(pinza, 150, 90, 20);  // SERVO 6
  delay(1000);

  // --- PASO 4: Levantar y volver al centro (SERVO 2, 1 y 3) ---
  Serial.println("Levantando y regresando...");
  moverSuave(hombro, 130, 90, 30); // SERVO 2
  moverSuave(base, 45, 90, 30);    // SERVO 1
  moverSuave(codo, 60, 90, 30);    // SERVO 3
  delay(1000);

  // --- PASO 5: Soltar objeto (SERVO 6) ---
  Serial.println("Soltando...");
  moverSuave(pinza, 90, 150, 20);  // SERVO 6
  delay(2000); 
}

// --- FUNCIÓN PARA MOVIMIENTOS SUAVES ---
void moverSuave(Servo &s, int inicio, int fin, int velocidad) {
  if (inicio < fin) {
    for (int p = inicio; p <= fin; p++) {
      s.write(p);
      delay(velocidad);
    }
  } else {
    for (int p = inicio; p >= fin; p--) {
      s.write(p);
      delay(velocidad);
    }
  }
}