Cómo crear un robot equilibrista con Arduino y PID

#include <Wire.h>

// --- Configuración PID (Ajustar estos valores es LA CLAVE) ---
double Kp = 20.0; 
double Ki = 0.8;  
double Kd = 0.5;  

// --- Pines del L298N ---
const int ENA = 9;
const int IN1 = 4;
const int IN2 = 5;
const int IN3 = 6;
const int IN4 = 7;
const int ENB = 10;

// Variables del IMU
long accelX, accelY, accelZ;
float gForceX, gForceY, gForceZ;
long gyroX, gyroY, gyroZ;
float rotX, rotY, rotZ;

float anguloGiro = 0, anguloAccel = 0, anguloActual = 0;
float error, errorAnterior = 0, sumaError = 0;

// Variables de tiempo para el PID
unsigned long tiempoActual, tiempoAnterior;
float dt;

// Punto de equilibrio (SetPoint)
// Ajustar si el robot tiende a irse hacia un lado mecánicamente
float setPoint = 0; 

void setup() {
  Serial.begin(9600);
  
  // Configurar Pines Motor
  pinMode(ENA, OUTPUT); pinMode(IN1, OUTPUT); pinMode(IN2, OUTPUT);
  pinMode(ENB, OUTPUT); pinMode(IN3, OUTPUT); pinMode(IN4, OUTPUT);
  
  // Iniciar I2C y MPU6050
  Wire.begin();
  Wire.beginTransmission(0x68);
  Wire.write(0x6B); // Registro de Power Management
  Wire.write(0);    // Despertar el MPU
  Wire.endTransmission(true);
  
  tiempoAnterior = millis();
}

void loop() {
  tiempoActual = millis();
  dt = (tiempoActual - tiempoAnterior) / 1000.0; // Tiempo en segundos
  
  // Si el bucle corre muy rápido, el dt puede ser 0. Forzamos un mínimo.
  if(dt < 0.01) return; 
  tiempoAnterior = tiempoActual;

  // 1. LEER DATOS (Función auxiliar definida abajo)
  leerMPU();

  // 2. FILTRO COMPLEMENTARIO
  // Ángulo del acelerómetro (Trigonometría básica)
  anguloAccel = atan2(accelY, accelZ) * 180 / PI;
  
  // Ángulo del giroscopio (Integración: velocidad * tiempo)
  // El 0.98 y 0.02 son los pesos del filtro.
  anguloActual = 0.98 * (anguloActual + rotX * dt) + 0.02 * anguloAccel;

  // 3. CÁLCULO PID
  error = setPoint - anguloActual;
  sumaError += error * dt;            // Término Integral
  // Limitamos la integral para evitar "windup" (que se acumule infinito)
  sumaError = constrain(sumaError, -200, 200); 
  
  float tasaError = (error - errorAnterior) / dt; // Término Derivativo
  
  // Salida del PID
  float salida = (Kp * error) + (Ki * sumaError) + (Kd * tasaError);
  
  errorAnterior = error;

  // 4. MOVER MOTORES
  if (abs(anguloActual) > 45) {
    sumaError = 0;
    errorAnterior = 0;
    detenerMotores();
    return;
  }

  moverMotores(salida);
}

void leerMPU() {
  Wire.beginTransmission(0x68);
  Wire.write(0x3B); // Registro inicial de datos del acelerómetro
  Wire.endTransmission(false);
  Wire.requestFrom(0x68, 6, true); 
  
  // Los valores vienen en dos bytes, hay que unirlos
  accelX = (Wire.read() << 8 | Wire.read());
  accelY = (Wire.read() << 8 | Wire.read());
  accelZ = (Wire.read() << 8 | Wire.read());

  // Ahora leemos giroscopio (Registro 0x43)
  Wire.beginTransmission(0x68);
  Wire.write(0x43);
  Wire.endTransmission(false);
  Wire.requestFrom(0x68, 6, true);
  
  gyroX = (Wire.read() << 8 | Wire.read());
  gyroY = (Wire.read() << 8 | Wire.read());
  gyroZ = (Wire.read() << 8 | Wire.read());
  
  // Convertir a grados/segundo (Divisor 131.0 es estándar para rango default)
  rotX = gyroX / 131.0;
}

void moverMotores(float velocidad) {
  // velocidad será un valor que puede ir de -infinito a +infinito
  // Mapeamos a PWM (0-255)
  
  int pwm = constrain(abs(velocidad), 0, 255);
  
  // Zona muerta: Los motores DC baratos no arrancan con PWM < 30-40
  // Si hay señal pero es baja, forzamos un mínimo para vencer la fricción
  if (pwm < 40 && pwm > 5) pwm = 40; 

  if (velocidad > 0) {
    // Hacia ADELANTE (Ajustar IN1/IN2 según vuestro cableado)
    digitalWrite(IN1, HIGH); digitalWrite(IN2, LOW);
    digitalWrite(IN3, HIGH); digitalWrite(IN4, LOW);
  } else {
    // Hacia ATRÁS
    digitalWrite(IN1, LOW); digitalWrite(IN2, HIGH);
    digitalWrite(IN3, LOW); digitalWrite(IN4, HIGH);
  }
  
  analogWrite(ENA, pwm);
  analogWrite(ENB, pwm);
}

void detenerMotores() {
  analogWrite(ENA, 0);
  analogWrite(ENB, 0);
}