/* * MECRO KIT - PROYECTO 12: BRAZO ROBÓTICO (1-DoF)
 * Objetivo: Control proporcional de posición y Mapeo de señales
 * Hardware: ESP32, Potenciómetro, Servomotor
 * Librerías: ESP32Servo
 */

#include <ESP32Servo.h>

// --- 1. DEFINICIONES DE HARDWARE ---
const int PIN_POT = 34;   // Entrada Analógica (ADC1_CH6)
const int PIN_SERVO = 13; // Salida PWM

// Objeto Servo
Servo miServo;

// --- 2. CONFIGURACIÓN DEL FILTRO (ANTI-JITTER) ---
// El ADC siempre tiene ruido. Promediamos lecturas para estabilizar.
const int NUM_LECTURAS = 10;
int lecturas[NUM_LECTURAS]; // Array circular
int indiceLectura = 0;
long total = 0;
int promedio = 0;

void setup() {
  Serial.begin(115200);
  
  // Configurar Servo
  // Los servos SG90 típicos responden a pulsos entre 500us y 2400us
  miServo.attach(PIN_SERVO, 500, 2400);
  
  // Inicializar el array de lecturas en 0
  for (int i = 0; i < NUM_LECTURAS; i++) {
    lecturas[i] = 0;
  }
  
  Serial.println("--- MECRO ROBOTIC ARM: ONLINE ---");
}

void loop() {
  // 1. FILTRO DE PROMEDIO MÓVIL
  // Restamos la última lectura guardada
  total = total - lecturas[indiceLectura];
  
  // Leemos el sensor nuevo
  lecturas[indiceLectura] = analogRead(PIN_POT);
  
  // Sumamos la nueva lectura
  total = total + lecturas[indiceLectura];
  
  // Avanzamos el índice
  indiceLectura = indiceLectura + 1;
  
  // Si llegamos al final del array, volvemos al principio (Buffer Circular)
  if (indiceLectura >= NUM_LECTURAS) {
    indiceLectura = 0;
  }
  
  // Calculamos el promedio
  promedio = total / NUM_LECTURAS;

  // 2. MAPEO DE SEÑAL (Matemáticas Lineales)
  // ADC ESP32: 0 a 4095 (12 bits)
  // Servo Ángulo: 0 a 180 grados
  int angulo = map(promedio, 0, 4095, 0, 180);
  
  // 3. ACTUAR (Mover el motor)
  miServo.write(angulo);
  
  // 4. TELEMETRÍA (Para debug)
  // Solo imprimimos cada cierto tiempo para no saturar el puerto
  static unsigned long lastPrint = 0;
  if (millis() - lastPrint > 100) {
    Serial.print("ADC Raw: "); Serial.print(analogRead(PIN_POT));
    Serial.print(" | ADC Smooth: "); Serial.print(promedio);
    Serial.print(" -> Angulo: "); Serial.println(angulo);
    lastPrint = millis();
  }
  
  delay(15); // Pequeña espera para permitir que el servo llegue físicamente
}