Funciona todo sin modulo_imu

modulo_imu.py

@@ -0,0 +1,296 @@
+"""
+modulo_imu.py — Sistema Vestibular del Robot
+=============================================
+Interfaz con el sensor MPU6050 via I2C en Raspberry Pi 5.
+
+Responsabilidades:
+  - Leer velocidad angular del eje X (Yaw) del giroscopio
+  - Integrar numéricamente para obtener ángulo absoluto
+  - Detectar y compensar el drift del giroscopio (bias)
+  - Exponer theta_rad (radianes) para fusión de sensores en SLAM
+
+Conexión hardware:
+  MPU6050 VCC  → Pin 1  (3.3V)
+  MPU6050 GND  → Pin 6  (GND)
+  MPU6050 SDA  → Pin 3  (GPIO2, I2C1 SDA)
+  MPU6050 SCL  → Pin 5  (GPIO3, I2C1 SCL)
+  MPU6050 AD0  → GND    (dirección I2C = 0x68)
+
+Dependencias:
+  pip install smbus2
+"""
+
+import smbus2
+import time
+import threading
+import logging
+import math
+
+logger = logging.getLogger(__name__)
+
+# ── Registros del MPU6050 ─────────────────────────────────────────────────────
+MPU6050_ADDR        = 0x68   # AD0 en GND → 0x68; AD0 en VCC → 0x69
+REG_PWR_MGMT_1      = 0x6B
+REG_SMPLRT_DIV      = 0x19
+REG_CONFIG          = 0x1A
+REG_GYRO_CONFIG     = 0x1B
+REG_ACCEL_CONFIG    = 0x1C
+REG_GYRO_XOUT_H     = 0x43
+REG_GYRO_YOUT_H     = 0x45
+REG_GYRO_ZOUT_H     = 0x47
+REG_ACCEL_ZOUT_H    = 0x3F
+REG_WHO_AM_I        = 0x75
+
+# ── Escalas del giroscopio ────────────────────────────────────────────────────
+# FS_SEL: 0 = ±250°/s, 1 = ±500°/s, 2 = ±1000°/s, 3 = ±2000°/s
+GYRO_FS_SEL         = 0       # ±250°/s → mejor resolución para robot lento
+GYRO_SCALE          = {0: 131.0, 1: 65.5, 2: 32.8, 3: 16.4}  # LSB/(°/s)
+
+ACCEL_FS_SEL        = 0       # ±2g
+ACCEL_SCALE         = {0: 16384.0, 1: 8192.0, 2: 4096.0, 3: 2048.0}  # LSB/g
+
+# ── Configuración del filtro DLPF ─────────────────────────────────────────────
+# DLPF_CFG 3 → Ancho de banda 44 Hz, delay 4.9ms — buen balance ruido/respuesta
+DLPF_CFG            = 3
+
+# ── Número de muestras para calibración de bias ───────────────────────────────
+CALIBRATION_SAMPLES = 500
+
+
+class IMU:
+    """
+    Interfaz de alto nivel con el MPU6050.
+
+    Uso básico:
+        imu = IMU(bus=1)
+        imu.iniciar()
+        ...
+        theta = imu.theta_rad   # ángulo Yaw integrado en radianes
+        imu.detener()
+    """
+
+    def __init__(self, bus: int = 1, direccion: int = MPU6050_ADDR,
+                 frecuencia_hz: float = 200.0):
+        """
+        Args:
+            bus:          Número del bus I2C (1 para RPi 5, pines 3/5).
+            direccion:    Dirección I2C del MPU6050 (0x68 o 0x69).
+            frecuencia_hz: Frecuencia de muestreo del bucle de integración.
+        """
+        self._bus_num      = bus
+        self._addr         = direccion
+        self._frecuencia   = frecuencia_hz
+        self._dt           = 1.0 / frecuencia_hz
+
+        self._bus: smbus2.SMBus | None = None
+
+        # Estado angular (acceso thread-safe)
+        self._lock         = threading.Lock()
+        self._theta_rad    = 0.0      # Yaw integrado
+        self._omega_x_rads = 0.0      # Velocidad angular actual (rad/s)
+        self._accel_z_ms2  = 0.0      # Aceleración lineal eje Z (m/s²)
+
+        # Calibración (bias en reposo)
+        self._bias_x_deg_s = 0.0
+        self._bias_accel_z_ms2 = 0.0
+
+        # Escala activa
+        self._escala       = GYRO_SCALE[GYRO_FS_SEL]
+        self._escala_accel = ACCEL_SCALE[ACCEL_FS_SEL]
+
+        # Hilo de lectura
+        self._hilo: threading.Thread | None = None
+        self._activo       = False
+
+    # ── Propiedades públicas (thread-safe) ────────────────────────────────────
+
+    @property
+    def theta_rad(self) -> float:
+        """Ángulo Yaw acumulado en radianes. Positivo = giro anti-horario."""
+        with self._lock:
+            return self._theta_rad
+
+    @property
+    def theta_deg(self) -> float:
+        """Ángulo Yaw acumulado en grados."""
+        return math.degrees(self.theta_rad)
+
+    @property
+    def omega_x_rads(self) -> float:
+        """Velocidad angular instantánea en el eje X (rad/s)."""
+        with self._lock:
+            return self._omega_x_rads
+
+    @property
+    def accel_z_ms2(self) -> float:
+        """Aceleración lineal instantánea en el eje Z (m/s²)."""
+        with self._lock:
+            return self._accel_z_ms2
+
+    # ── Inicialización ────────────────────────────────────────────────────────
+
+    def iniciar(self) -> None:
+        """
+        Abre el bus I2C, configura el MPU6050, calibra el bias
+        y lanza el hilo de integración en segundo plano.
+        """
+        logger.info("IMU: Abriendo bus I2C-%d, dirección 0x%02X", self._bus_num, self._addr)
+        self._bus = smbus2.SMBus(self._bus_num)
+        self._verificar_chip()
+        self._configurar_chip()
+        logger.info("IMU: Calibrando bias de giroscopio y acelerómetro (%d muestras)...", CALIBRATION_SAMPLES)
+        self._calibrar_bias()
+        logger.info("IMU: Bias Eje-X = %.4f °/s | Bias Accel Z = %.4f m/s²", self._bias_x_deg_s, self._bias_accel_z_ms2)
+
+        self._activo = True
+        self._hilo = threading.Thread(target=self._bucle_integracion,
+                                      name="hilo-imu", daemon=True)
+        self._hilo.start()
+        logger.info("IMU: Hilo de integración iniciado a %.0f Hz", self._frecuencia)
+
+    def detener(self) -> None:
+        """Detiene el hilo de lectura y cierra el bus I2C."""
+        self._activo = False
+        if self._hilo:
+            self._hilo.join(timeout=2.0)
+        if self._bus:
+            self._bus.close()
+        logger.info("IMU: Detenida.")
+
+    def resetear_angulo(self, valor_rad: float = 0.0) -> None:
+        """
+        Reestablece el ángulo Yaw integrado a un valor arbitrario.
+        Útil cuando el SLAM cierra un loop y corrige la pose.
+        """
+        with self._lock:
+            self._theta_rad = valor_rad
+
+    # ── Configuración del chip ────────────────────────────────────────────────
+
+    def _verificar_chip(self) -> None:
+        who = self._bus.read_byte_data(self._addr, REG_WHO_AM_I)
+        if who != 0x68:
+            raise RuntimeError(
+                f"IMU: WHO_AM_I=0x{who:02X} — ¿MPU6050 conectado y AD0 en GND?"
+            )
+        logger.debug("IMU: WHO_AM_I OK (0x68)")
+
+    def _configurar_chip(self) -> None:
+        # Despertar el chip (sale del modo sleep)
+        self._bus.write_byte_data(self._addr, REG_PWR_MGMT_1, 0x00)
+        time.sleep(0.1)
+
+        # Seleccionar reloj del giroscopio eje X (más estable que el oscilador interno)
+        self._bus.write_byte_data(self._addr, REG_PWR_MGMT_1, 0x01)
+
+        # Sample Rate Divider: SMPLRT_DIV=0 → Fsample = 8 kHz / (1+0) = 8 kHz
+        # (El hilo de Python muestrea a 200 Hz; el chip siempre está listo)
+        self._bus.write_byte_data(self._addr, REG_SMPLRT_DIV, 0x00)
+
+        # Filtro pasa-bajas digital (DLPF)
+        self._bus.write_byte_data(self._addr, REG_CONFIG, DLPF_CFG)
+
+        # Rango del giroscopio y acelerómetro
+        self._bus.write_byte_data(self._addr, REG_GYRO_CONFIG, GYRO_FS_SEL << 3)
+        self._bus.write_byte_data(self._addr, REG_ACCEL_CONFIG, ACCEL_FS_SEL << 3)
+
+        logger.debug("IMU: Chip configurado. Rango=±%s°/s, DLPF=%d",
+                     {0: 250, 1: 500, 2: 1000, 3: 2000}[GYRO_FS_SEL], DLPF_CFG)
+
+    # ── Lectura de registros ──────────────────────────────────────────────────
+
+    def _leer_gyro_x_raw(self) -> int:
+        """Lee los 2 bytes del giroscopio eje X y devuelve el entero con signo."""
+        high = self._bus.read_byte_data(self._addr, REG_GYRO_XOUT_H)
+        low  = self._bus.read_byte_data(self._addr, REG_GYRO_XOUT_H + 1)
+        valor = (high << 8) | low
+        # Convertir a complemento a dos
+        if valor >= 0x8000:
+            valor -= 0x10000
+        return valor
+
+    def _leer_gyro_x_deg_s(self) -> float:
+        """Devuelve la velocidad angular del eje X en grados/segundo."""
+        return self._leer_gyro_x_raw() / self._escala
+
+    def _leer_accel_z_raw(self) -> int:
+        """Lee los 2 bytes del acelerómetro eje Z y devuelve el entero con signo."""
+        high = self._bus.read_byte_data(self._addr, REG_ACCEL_ZOUT_H)
+        low  = self._bus.read_byte_data(self._addr, REG_ACCEL_ZOUT_H + 1)
+        valor = (high << 8) | low
+        if valor >= 0x8000:
+            valor -= 0x10000
+        return valor
+
+    def _leer_accel_z_ms2_crudo(self) -> float:
+        """Devuelve la aceleración cruda del eje Z en m/s²."""
+        g_force = self._leer_accel_z_raw() / self._escala_accel
+        return g_force * 9.80665
+
+    # ── Calibración ───────────────────────────────────────────────────────────
+
+    def _calibrar_bias(self) -> None:
+        """
+        Estima el drift (offset) del giroscopio y la gravedad estática del acelerómetro en reposo.
+        El robot DEBE estar completamente inmóvil durante este proceso.
+        """
+        acumulador_gyro = 0.0
+        acumulador_accel = 0.0
+        for _ in range(CALIBRATION_SAMPLES):
+            acumulador_gyro += self._leer_gyro_x_deg_s()
+            acumulador_accel += self._leer_accel_z_ms2_crudo()
+            time.sleep(0.002)  # ~500 Hz durante calibración
+        self._bias_x_deg_s = acumulador_gyro / CALIBRATION_SAMPLES
+        self._bias_accel_z_ms2 = acumulador_accel / CALIBRATION_SAMPLES
+
+    # ── Bucle de integración ──────────────────────────────────────────────────
+
+    def _bucle_integracion(self) -> None:
+        """
+        Hilo principal de integración.
+        Lee ω_x, resta el bias, integra numéricamente y actualiza theta_rad.
+
+              Δθ = -(ω_x − bias) × Δt
+              θ_actual = θ_anterior + Δθ
+        """
+        intervalo = self._dt
+        siguiente_tick = time.perf_counter() + intervalo
+
+        while self._activo:
+            ahora = time.perf_counter()
+
+            try:
+                # Giroscopio (sentido del ángulo invertido)
+                omega_x_deg_s = self._leer_gyro_x_deg_s() - self._bias_x_deg_s
+                omega_x_rads  = -math.radians(omega_x_deg_s)
+                delta_theta   = omega_x_rads * intervalo
+
+                # Acelerómetro
+                accel_z = self._leer_accel_z_ms2_crudo() - self._bias_accel_z_ms2
+
+                with self._lock:
+                    self._theta_rad    += delta_theta
+                    self._omega_x_rads  = omega_x_rads
+                    self._accel_z_ms2   = accel_z
+
+            except Exception as e:
+                logger.warning("IMU: Error de lectura I2C: %s", e)
+
+            # Espera precisa hasta el siguiente tick
+            siguiente_tick += intervalo
+            pausa = siguiente_tick - time.perf_counter()
+            if pausa > 0:
+                time.sleep(pausa)
+
+    # ── Diagnóstico ───────────────────────────────────────────────────────────
+
+    def diagnostico(self) -> dict:
+        """Devuelve un snapshot del estado actual para logging/debug."""
+        with self._lock:
+            return {
+                "theta_deg":    math.degrees(self._theta_rad),
+                "theta_rad":    self._theta_rad,
+                "omega_x_rads": self._omega_x_rads,
+                "bias_x_deg_s": self._bias_x_deg_s,
+                "activo":       self._activo,
+            }