🧭 IMU y encoders: saber dónde está

A IMU (unidad de medición inercial) es un chip que detecta el movimiento sin mirar hacia afuera. La MPU-6050 tiene dos sensores de tres ejes cada uno. El acelerómetro mide la aceleración, incluida la gravedad: cuando está quieto y nivelado, muestra 1 g apuntando hacia abajo, por eso sirve para conocer la inclinación. El giroscopio mide la velocidad de giro en cada eje, en grados por segundo; al sumar esa velocidad a lo largo del tiempo, sabes cuánto giró el robot.

Cómo leer: las flechas azules son los tres ejes, siempre fijos al chip. La flecha ámbar es la gravedad que detecta el acelerómetro; la curva cian es el giro alrededor de Z, que mide el giroscopio cuando el robot gira. Los dos cuadros resumen el punto fuerte y el punto débil de cada sensor: uno complementa al otro, y eso es lo que aprovecha el tema 3.

Por qué aprender

Girar exactamente 90°, detectar que subiste por una rampa o detectar que el robot se volcó son tareas de la IMU. Sin ella, el robot solo sabe cuánto giraron las ruedas, y las ruedas patinan.

Conceptos clave

La MPU-6050 se comunica por I2C, el bus de dos cables que conociste en el 1.4. Son cuatro conexiones: VCC a 3V3, GND a GND, SDA a GPIO 21 y SCL a GPIO 22. En lugar de una biblioteca lista para usar, te comunicarás directamente con el chip: escribirás en un registro para despertarlo y leerás 14 bytes de una vez, con aceleración, temperatura y giro.

Cómo leer: a la izquierda, los cuatro cables; rojo es alimentación, gris es tierra, azul y cian son los datos de I2C. A la derecha, el orden en que llegan los 14 bytes cuando solicitas la lectura a partir del registro 0x3B. El código lee en ese mismo orden.

Objetivo: despertar la MPU-6050 y mostrar aceleración (g) y giro (°/s) diez veces por segundo. Funciona en Wokwi, que incluye la MPU-6050 en la lista de componentes.

// MPU-6050 por I2C, sin biblioteca adicional (paquete esp32 3.x)
#include <Wire.h>
const int MPU = 0x68;                     // dirección con AD0 a GND

int16_t ler16() {                          // une dos bytes: primero el alto
  uint8_t alto = Wire.read();
  uint8_t baixo = Wire.read();
  return (int16_t)((alto << 8) | baixo);
}

void setup() {
  Serial.begin(115200);
  Wire.begin(21, 22);                      // SDA, SCL
  Wire.beginTransmission(MPU);
  Wire.write(0x6B); Wire.write(0);         // registro de energía = 0: sale del modo de suspensión
  if (Wire.endTransmission() != 0) {
    Serial.println("MPU-6050 no respondió: revisa los cables y la dirección");
    while (true) delay(1000);
  }
  Serial.println("MPU-6050 lista");
}

void loop() {
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);                        // empieza en la aceleración X
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 14);
  float ax = ler16() / 16384.0;
  float ay = ler16() / 16384.0;
  float az = ler16() / 16384.0;
  ler16();                                 // temperatura: se ignora aquí
  float gx = ler16() / 131.0;
  float gy = ler16() / 131.0;
  float gz = ler16() / 131.0;
  Serial.printf("ax:%.2f,ay:%.2f,az:%.2f,gx:%.1f,gy:%.1f,gz:%.1f\n", ax, ay, az, gx, gy, gz);
  delay(100);
}

Por qué aprender

Leer los registros directamente muestra lo que las bibliotecas ocultan y funciona igual para casi cualquier sensor I2C: dirección, registro, cantidad de bytes, conversión de unidades. Cuando cambies de IMU, sabrás dónde buscar en la hoja de datos.

Conceptos clave

Puedes calcular la inclinación de dos maneras. Con el acelerómetro, usando trigonometría sobre la gravedad: es correcto en promedio, pero da saltos con cada sacudida. Con el giroscopio, sumando el giro: es fluido y rápido, pero la deriva aleja el resultado. El filtro complementario combina ambos: confía un 98% en el giroscopio en cada paso y usa un 2% del acelerómetro para devolver el ángulo a su valor real.

Cómo leer: el camino de arriba (cian) sigue el movimiento rápido; el de abajo (azul) corrige la deriva lentamente. Los pesos suman 1. La flecha punteada muestra que el filtro tiene memoria: cada nuevo ángulo parte del anterior. Más peso en el giroscopio hace que el ángulo sea más fluido; más peso en el acelerómetro corrige más rápido, con más ruido.

Objetivo: calcular la inclinación hacia adelante (cabeceo) de tres maneras y ver las tres en el Serial Plotter: solo acelerómetro, solo giroscopio y filtrado. El bucle se ejecuta cada 10 ms.

// Inclinación con filtro complementario (MPU-6050, paquete esp32 3.x)
#include <Wire.h>
const int MPU = 0x68;
const float ALFA = 0.98;                  // confianza en el giroscopio
const float DT = 0.01;                    // 10 ms por paso
float offsetGy = 0, angGiro = 0, angFiltro = 0;

int16_t ler16() { uint8_t a = Wire.read(); uint8_t b = Wire.read(); return (int16_t)((a << 8) | b); }

void lerMPU(float &ax, float &ay, float &az, float &gy) {
  Wire.beginTransmission(MPU); Wire.write(0x3B); Wire.endTransmission(false);
  Wire.requestFrom(MPU, 14);
  ax = ler16() / 16384.0; ay = ler16() / 16384.0; az = ler16() / 16384.0;
  ler16(); ler16();                        // temperatura y giro X: ignorados
  gy = ler16() / 131.0; ler16();           // giro Y (usado) y giro Z
}

void setup() {
  Serial.begin(115200);
  Wire.begin(21, 22);
  Wire.beginTransmission(MPU); Wire.write(0x6B); Wire.write(0); Wire.endTransmission();
  Serial.println("Calibrando: no muevas el robot...");
  float ax, ay, az, gy, soma = 0;
  for (int i = 0; i < 500; i++) { lerMPU(ax, ay, az, gy); soma += gy; delay(2); }
  offsetGy = soma / 500;                   // el giro "en reposo" se convierte en el cero
  lerMPU(ax, ay, az, gy);
  angFiltro = angGiro = atan2(-ax, sqrt(ay * ay + az * az)) * 180 / PI;
}

void loop() {
  static unsigned long ultimo = micros();
  if (micros() - ultimo < 10000) return;   // espera a completar 10 ms
  ultimo += 10000;
  float ax, ay, az, gy;
  lerMPU(ax, ay, az, gy);
  gy -= offsetGy;
  float angAcel = atan2(-ax, sqrt(ay * ay + az * az)) * 180 / PI;
  angGiro += gy * DT;                                          // solo giroscopio: deriva
  angFiltro = ALFA * (angFiltro + gy * DT) + (1 - ALFA) * angAcel;
  Serial.printf("acel:%.1f,giro:%.1f,filtro:%.1f\n", angAcel, angGiro, angFiltro);
}

Por qué aprender

Cinco líneas de cálculo resuelven el problema que hace caer a los robots equilibristas y a los drones caseros. El filtro de Kalman, que encontrarás en proyectos más grandes, sigue la misma idea de combinar un sensor rápido con uno que no deriva, con pesos que se ajustan solos.

Conceptos clave

Un encoder convierte el giro en pulsos. En el motor TT, un disco con 20 orificios pasa entre un LED y un sensor de luz: cada orificio es un pulso. En el motor N20 con encoder, un imán en el eje pasa por dos sensores Hall. Los dos sensores, llamados canales A y B, están desfasados: el que pulsa primero indica el sentido del giro. Esto se llama cuadratura.

Cómo leer: las dos ondas tienen el mismo ritmo, pero B se retrasa un cuarto de ciclo. En las líneas ámbar, A subió primero: la rueda gira hacia adelante. Si la rueda invierte el sentido, B empieza a cambiar primero. Contar cada flanco de A (subida y bajada) duplica la resolución sin costo.

Objetivo: contar los pulsos de las dos ruedas con una interrupción y mostrar la rotación en RPM cada 200 ms. Encoders: izquierdo A 18 / B 19, derecho A 34 / B 35 (34 y 35 no tienen pull-up interno; los módulos Hall del N20 suelen tenerlo).

// Dos encoders en cuadratura con interrupción (paquete esp32 3.x)
const int ENC_E_A = 18, ENC_E_B = 19;      // rueda izquierda
const int ENC_D_A = 34, ENC_D_B = 35;      // rueda derecha
const float PULSOS_VOLTA = 1400;          // N20 1:100 contando los 2 flancos de A. Mide el tuyo (tema 6)
volatile long pulsosE = 0, pulsosD = 0;

void IRAM_ATTR isrE() { if (digitalRead(ENC_E_A) == digitalRead(ENC_E_B)) pulsosE++; else pulsosE--; }
void IRAM_ATTR isrD() { if (digitalRead(ENC_D_A) == digitalRead(ENC_D_B)) pulsosD--; else pulsosD++; }  // invertido

void setup() {
  Serial.begin(115200);
  pinMode(ENC_E_A, INPUT_PULLUP); pinMode(ENC_E_B, INPUT_PULLUP);
  pinMode(ENC_D_A, INPUT); pinMode(ENC_D_B, INPUT);
  attachInterrupt(digitalPinToInterrupt(ENC_E_A), isrE, CHANGE);
  attachInterrupt(digitalPinToInterrupt(ENC_D_A), isrD, CHANGE);
  Serial.println("Encoders listos: gira las ruedas con la mano");
}

void loop() {
  static long antE = 0, antD = 0;
  delay(200);
  noInterrupts(); long e = pulsosE, d = pulsosD; interrupts();   // copia sin interrupciones
  float rpmE = (e - antE) / PULSOS_VOLTA / 0.2 * 60;
  float rpmD = (d - antD) / PULSOS_VOLTA / 0.2 * 60;
  antE = e; antD = d;
  Serial.printf("pulsosE:%ld,pulsosD:%ld,rpmE:%.1f,rpmD:%.1f\n", e, d, rpmE, rpmD);
}

Por qué aprender

Sin encoder, "avanzar 1 metro" se convierte en "encender el motor por 2 segundos y cruzar los dedos". Con él, mides la velocidad real (la entrada del PID de 2.3) y la distancia recorrida (la base de la odometría del siguiente tema).

Conceptos clave

Odometría es estimar la posición sumando pequeños desplazamientos. Cada 50 ms calculas cuánto avanzó cada rueda (pulsos ÷ pulsos por vuelta × perímetro de la rueda). El promedio de las dos indica cuánto avanzó el centro del robot; la diferencia entre ellas, dividida por la distancia entre las ruedas, indica cuánto giró. Con eso actualizas x, y y θ (la dirección, en radianes).

Cómo leer: La flecha azul es la parte delantera del robot y apunta en la dirección θ, medida desde el eje x. La regla ámbar L es la distancia entre las ruedas. Las cinco líneas del cuadro son todo el algoritmo: si la rueda derecha avanzó más que la izquierda, Δθ es positivo y el robot giró a la izquierda.

Objetivo: estimar x, y (en centímetros) y θ (en grados) empujando el robot con la mano. Los mismos pines del tema 4. Ajusta el radio de la rueda y la distancia entre ruedas de tu chasis.

// Odometry of a differential-drive robot with two encoders (esp32 package 3.x)
const int ENC_E_A = 18, ENC_E_B = 19, ENC_D_A = 34, ENC_D_B = 35;
const float PULSOS_VOLTA = 1400;           // measure yours (topic 6)
const float RAIO = 0.0215;                 // wheel radius in meters (43 mm wheel)
const float BASE = 0.130;                  // distance between the wheels in meters
const float M_POR_PULSO = 2 * PI * RAIO / PULSOS_VOLTA;
volatile long pulsosE = 0, pulsosD = 0;
float x = 0, y = 0, theta = 0;

void IRAM_ATTR isrE() { if (digitalRead(ENC_E_A) == digitalRead(ENC_E_B)) pulsosE++; else pulsosE--; }
void IRAM_ATTR isrD() { if (digitalRead(ENC_D_A) == digitalRead(ENC_D_B)) pulsosD--; else pulsosD++; }

void setup() {
  Serial.begin(115200);
  pinMode(ENC_E_A, INPUT_PULLUP); pinMode(ENC_E_B, INPUT_PULLUP);
  pinMode(ENC_D_A, INPUT); pinMode(ENC_D_B, INPUT);
  attachInterrupt(digitalPinToInterrupt(ENC_E_A), isrE, CHANGE);
  attachInterrupt(digitalPinToInterrupt(ENC_D_A), isrD, CHANGE);
}

void loop() {
  static long antE = 0, antD = 0;
  static unsigned long ultimoPrint = 0;
  delay(50);                                  // 50 ms step
  noInterrupts(); long e = pulsosE, d = pulsosD; interrupts();
  float dE = (e - antE) * M_POR_PULSO;
  float dD = (d - antD) * M_POR_PULSO;
  antE = e; antD = d;
  float dist = (dE + dD) / 2;
  float dTheta = (dD - dE) / BASE;
  x += dist * cos(theta + dTheta / 2);
  y += dist * sin(theta + dTheta / 2);
  theta = atan2(sin(theta + dTheta), cos(theta + dTheta));   // keeps the value between -180° and 180°
  if (millis() - ultimoPrint >= 250) {
    ultimoPrint = millis();
    Serial.printf("x=%.1f cm  y=%.1f cm  theta=%.1f degrees\n", x * 100, y * 100, theta * 180 / PI);
  }
}

Por qué aprender

Volver a la base, avanzar hasta un punto, dibujar un cuadrado en el suelo: todo eso necesita x, y y θ. Eso es exactamente lo que micro-ROS de 4.3 publica como odometría para que ROS 2 construya mapas.

Conceptos clave

La odometría nunca es exacta, solo «casi exacta». Cada paso arrastra un pequeño error: la rueda patinó, el diámetro real es 1 mm mayor, se perdió un pulso. Como la posición es la suma de todos los pasos, estos errores se acumulan, y el error de dirección es el peor: 2° de error al principio se convierten en 3,5 cm de desviación por cada metro recorrido. La forma de convivir con esto es calibrar bien y corregir de vez en cuando con algo que no derive.

Cómo leer: El cuadrado azul es lo que el robot hizo en realidad; la línea cian es lo que sumó la odometría. En el primer tramo casi no hay diferencia. En cada giro, un pequeño error de ángulo deforma todos los tramos siguientes. Por eso la prueba de calibración usa vueltas completas: el error se hace grande y es fácil de medir.

Calibración en 4 pasos

Por qué aprender

Saber que la posición «se desliza» cambia la forma de diseñar: las tareas cortas pueden confiar en la odometría; las tareas largas necesitan una referencia externa, como la línea en el suelo de 2.6, la cámara de la Ruta 3 o el LiDAR con SLAM de la Ruta 4, que existe precisamente para corregir este error.

Conceptos clave