Imu y velocidad funcionan a medias

main.py

@@ -6,6 +6,7 @@ import math
 from modulo_esp32 import ChasisESP32
 from modulo_camara import CamaraD415
 from modulo_slam import EvasorObstaculos
+from modulo_imu import IMU
 
 # ── Constantes de velocidad ────────────────────────────────────────────────────
 VEL_NORMAL = 0.50    # m/s en camino libre
@@ -13,12 +14,13 @@ VEL_LENTA  = 0.25    # m/s en precaución
 VEL_GIRO   = 1.50    # rad/s al girar
 
 # ── Mapa 2D local ──────────────────────────────────────────────────────────────
-MAPA_W, MAPA_H = 500, 500
+MAPA_W, MAPA_H = 700, 700
-ESCALA  = 200          # píxeles por metro
+ESCALA  = 150          # píxeles por metro
 ORIGEN  = (MAPA_W // 2, MAPA_H // 2)
 
 # ── Objetos globales ───────────────────────────────────────────────────────────
-robot      = ChasisESP32(puerto='/dev/ttyAMA0')
+imu        = IMU(bus=1)                        # MPU6050 vía I2C
+robot      = ChasisESP32(puerto='/dev/ttyAMA0', imu=imu)
 camara     = CamaraD415()
 evasor     = EvasorObstaculos()
 ejecutando = True
@@ -26,8 +28,47 @@ ejecutando = True
 # Flag de calibración en curso (evita que el autopiloto interfiera)
 calibrando = False
 
-# ── Mapa acumulado ─────────────────────────────────────────────────────────────
+# Trayectoria acumulada: lista de (px, py, velocidad_ms) para colorear por velocidad
-mapa_fondo = np.zeros((MAPA_H, MAPA_W, 3), dtype=np.uint8)
+trayectoria: list = []
+
+# Mapa de fondo estático (cuadrícula, solo se dibuja una vez y se cachea)
+_mapa_base: np.ndarray | None = None
+
+
+def _construir_base() -> np.ndarray:
+    """Dibuja el fondo con cuadrícula y etiquetas de metros. Se llama una sola vez."""
+    base = np.full((MAPA_H, MAPA_W, 3), (18, 20, 26), dtype=np.uint8)  # fondo casi negro
+
+    # Cuadrícula menor (cada 0.5 m)
+    paso_menor = ESCALA // 2
+    for i in range(0, MAPA_W, paso_menor):
+        cv2.line(base, (i, 0), (i, MAPA_H), (35, 40, 50), 1)
+        cv2.line(base, (0, i), (MAPA_W, i), (35, 40, 50), 1)
+
+    # Cuadrícula mayor (cada 1 m) con etiqueta
+    for i in range(0, MAPA_W, ESCALA):
+        cv2.line(base, (i, 0), (i, MAPA_H), (55, 65, 80), 1)
+        cv2.line(base, (0, i), (MAPA_W, i), (55, 65, 80), 1)
+        # Etiqueta de metros en eje X
+        metros_x = (i - ORIGEN[0]) / ESCALA
+        if metros_x != 0:
+            label = f"{metros_x:+.0f}m"
+            cv2.putText(base, label, (i + 2, ORIGEN[1] - 4),
+                        cv2.FONT_HERSHEY_SIMPLEX, 0.3, (70, 80, 100), 1)
+        # Etiqueta de metros en eje Y
+        metros_y = -(i - ORIGEN[1]) / ESCALA
+        if metros_y != 0:
+            label = f"{metros_y:+.0f}m"
+            cv2.putText(base, label, (ORIGEN[0] + 4, i - 2),
+                        cv2.FONT_HERSHEY_SIMPLEX, 0.3, (70, 80, 100), 1)
+
+    # Cruz de origen
+    cv2.line(base, (ORIGEN[0], 0), (ORIGEN[0], MAPA_H), (80, 100, 130), 1)
+    cv2.line(base, (0, ORIGEN[1]), (MAPA_W, ORIGEN[1]), (80, 100, 130), 1)
+    cv2.drawMarker(base, ORIGEN, (100, 180, 255), cv2.MARKER_CROSS, 16, 2)
+    cv2.putText(base, "O", (ORIGEN[0] + 6, ORIGEN[1] - 6),
+                cv2.FONT_HERSHEY_SIMPLEX, 0.4, (100, 180, 255), 1)
+    return base
 
 
 # ── Hilo de hardware ──────────────────────────────────────────────────────────
@@ -55,52 +96,84 @@ def cmd_a_velocidad(cmd: str) -> tuple[float, float]:
     return tabla.get(cmd, (0.0, 0.0))
 
 
-# ── Dibujar mapa 2D ───────────────────────────────────────────────────────────
+# ── Dibujar mapa 2D ──────────────────────────────────────────────────────────────
 def dibujar_mapa(x: float, y: float, theta: float) -> np.ndarray:
-    global mapa_fondo
+    global _mapa_base
 
+    # Construir fondo solo la primera vez
+    if _mapa_base is None:
+        _mapa_base = _construir_base()
+
+    # Posición en píxeles
     px = int(ORIGEN[0] + x * ESCALA)
     py = int(ORIGEN[1] - y * ESCALA)
-    px_c = max(0, min(MAPA_W - 1, px))
+    px_c = max(1, min(MAPA_W - 2, px))
-    py_c = max(0, min(MAPA_H - 1, py))
+    py_c = max(1, min(MAPA_H - 2, py))
 
-    cv2.circle(mapa_fondo, (px_c, py_c), 2, (80, 80, 80), -1)
+    # Guardar punto de trayectoria con velocidad actual
-    mapa = mapa_fondo.copy()
+    trayectoria.append((px_c, py_c, robot.velocidad_ms))
 
-    # Cuadrícula (cada metro)
+    # ── Componer frame ─────────────────────────────────────────────────
-    for i in range(0, MAPA_W, ESCALA):
+    mapa = _mapa_base.copy()
-        cv2.line(mapa, (i, 0), (i, MAPA_H), (30, 30, 30), 1)
-        cv2.line(mapa, (0, i), (MAPA_W, i), (30, 30, 30), 1)
-    cv2.drawMarker(mapa, ORIGEN, (60, 60, 60), cv2.MARKER_CROSS, 12, 1)
 
-    # Robot
+    # Trayectoria con degradado temporal: gris viejo → cian brillante reciente
-    radio = 10
+    n = len(trayectoria)
-    cv2.circle(mapa, (px_c, py_c), radio, (0, 200, 255), 2)
+    for idx, (tx, ty, vel) in enumerate(trayectoria):
-    fx = int(px_c + math.cos(theta) * radio * 1.6)
+        progreso = idx / max(n - 1, 1)          # 0.0 (antiguo) → 1.0 (reciente)
-    fy = int(py_c - math.sin(theta) * radio * 1.6)
+        # Color: de (50,50,80) gris-azul a (0,255,180) cian-verde
-    cv2.arrowedLine(mapa, (px_c, py_c), (fx, fy), (0, 200, 255), 2, tipLength=0.4)
+        b = int(80  + progreso * (255 - 80))
+        g = int(50  + progreso * (255 - 50))
+        r = int(50  + progreso * (0   - 50))
+        radio_pt = 3 if progreso > 0.98 else (2 if progreso > 0.8 else 1)
+        cv2.circle(mapa, (tx, ty), radio_pt, (b, g, r), -1)
 
-    # Banner de calibración en curso
+    # ── Robot ──────────────────────────────────────────────────────────────
+    radio = 12
+    # Sombra
+    cv2.circle(mapa, (px_c + 2, py_c + 2), radio + 2, (0, 0, 0), -1)
+    # Cuerpo amarillo-naranja
+    cv2.circle(mapa, (px_c, py_c), radio, (0, 200, 255), -1)
+    cv2.circle(mapa, (px_c, py_c), radio, (0, 255, 255), 2)
+    # Flecha de dirección (más larga y gruesa)
+    fx = int(px_c + math.cos(theta) * radio * 2.2)
+    fy = int(py_c - math.sin(theta) * radio * 2.2)
+    cv2.arrowedLine(mapa, (px_c, py_c), (fx, fy), (255, 255, 255), 2, tipLength=0.35)
+
+    # ── Banner de calibración ─────────────────────────────────────────────────
     if calibrando:
-        cv2.rectangle(mapa, (0, 0), (MAPA_W, 28), (0, 60, 120), -1)
+        cv2.rectangle(mapa, (0, 0), (MAPA_W, 30), (0, 50, 110), -1)
-        cv2.putText(mapa, "⚙ CALIBRANDO — espera la trama K:",
+        cv2.putText(mapa, "  CALIBRANDO — manten robot en reposo",
-                    (8, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.52, (0, 220, 255), 1)
+                    (8, 22), cv2.FONT_HERSHEY_SIMPLEX, 0.55, (0, 220, 255), 1)
 
-    # HUD del evasor
+    # ── HUD inferior ────────────────────────────────────────────────────────────
-    evasor.dibujar_estado(mapa)
+    # Panel semitransparente inferior
+    panel_y = MAPA_H - 72
+    overlay = mapa.copy()
+    cv2.rectangle(overlay, (0, panel_y), (MAPA_W, MAPA_H), (10, 12, 18), -1)
+    cv2.addWeighted(overlay, 0.80, mapa, 0.20, 0, mapa)
+    cv2.line(mapa, (0, panel_y), (MAPA_W, panel_y), (60, 80, 110), 1)
 
-    # VEL_MAX actual (para diagnóstico visual)
+    # Fila 1: pose del robot
-    vmax_txt = (f"VelMax  izq={robot.vel_max_izq:.0f}  "
+    pose_txt = (f"x={x:+.2f}m   y={y:+.2f}m   θ={math.degrees(theta):+.1f}°")
-                f"der={robot.vel_max_der:.0f}  ticks/s")
+    cv2.putText(mapa, pose_txt,
+                (10, panel_y + 18), cv2.FONT_HERSHEY_SIMPLEX, 0.47,
+                (200, 220, 255), 1, cv2.LINE_AA)
+
+    # Fila 2: velocidad + IMU
+    vel_txt = (f"V={robot.velocidad_ms * 100:.1f} cm/s   "
+               f"IMU={robot.theta_imu_deg:+.1f}°")
+    cv2.putText(mapa, vel_txt,
+                (10, panel_y + 38), cv2.FONT_HERSHEY_SIMPLEX, 0.47,
+                (100, 255, 180), 1, cv2.LINE_AA)
+
+    # Fila 3: vel_max motores + estado evasor
+    ev = evasor.ultimo_estado
+    cmd_txt = ev.get("comando", "---") if ev else "---"
+    vmax_txt = (f"VelMax izq={robot.vel_max_izq:.0f} der={robot.vel_max_der:.0f} t/s   "
+                f"[{cmd_txt}]")
     cv2.putText(mapa, vmax_txt,
-                (8, MAPA_H - 24), cv2.FONT_HERSHEY_SIMPLEX, 0.38,
+                (10, panel_y + 56), cv2.FONT_HERSHEY_SIMPLEX, 0.38,
-                (80, 160, 80), 1, cv2.LINE_AA)
+                (140, 160, 140), 1, cv2.LINE_AA)
-
-    # Coordenadas
-    cv2.putText(mapa,
-                f"x={x:.2f}m  y={y:.2f}m  θ={math.degrees(theta):.1f}°",
-                (8, MAPA_H - 8), cv2.FONT_HERSHEY_SIMPLEX, 0.42,
-                (140, 140, 140), 1, cv2.LINE_AA)
 
     return mapa
 
@@ -129,6 +202,11 @@ def main():
     print("\n🚀 INICIANDO SISTEMA AUTÓNOMO...")
     print("Teclas (ventana Mapa): [a] Auto | [m] Manual | [c] Calibrar | [q] Salir")
 
+    # ── Iniciar IMU (calibra bias en reposo ~1 s) ─────────────────────────────
+    print("🧐 Calibrando IMU (mantener robot en reposo)...")
+    imu.iniciar()
+    print("✅ IMU lista.")
+
     hilo = threading.Thread(target=hilo_hardware, daemon=True)
     hilo.start()
 
@@ -137,12 +215,8 @@ def main():
 
     try:
         while True:
-            escaner, frame_video = camara.obtener_datos()
+            # Cámara: solo se usa para el evasor de obstáculos, sin mostrar video
-
+            escaner, _ = camara.obtener_datos()
-            # Vídeo de profundidad
-            if frame_video is not None:
-                frame_video[235:245, :, :] = frame_video[240, :, :]
-                cv2.imshow("Vision RealSense D415", frame_video)
 
             # Decisión del evasor
             estado = evasor.decidir_movimiento(escaner)
@@ -199,6 +273,7 @@ def main():
         if hilo_calib and hilo_calib.is_alive():
             hilo_calib.join(timeout=2.0)
         robot.enviar_velocidad(0.0, 0.0)
+        imu.detener()
         camara.cerrar()
         cv2.destroyAllWindows()
         print("\n🔌 Apagado.")

modulo_esp32.py

@@ -1,5 +1,10 @@
 import serial
 import math
+import logging
+
+logger = logging.getLogger(__name__)
+
+
 
 
 class ChasisESP32:
@@ -29,7 +34,16 @@ class ChasisESP32:
     # M1=FL, M2=FR, M3=RL, M4=RR
     _VEL_MAX_TICKS = [200.0, 200.0, 200.0, 200.0]
 
-    def __init__(self, puerto: str = '/dev/ttyAMA0', baudrate: int = 921600):
+    def __init__(self, puerto: str = '/dev/ttyAMA0', baudrate: int = 921600,
+                 imu=None):
+        """
+        Args:
+            puerto:   Puerto serie del ESP32.
+            baudrate: Velocidad de comunicación.
+            imu:      Instancia de modulo_imu.IMU ya iniciada, o None.
+                      - Rotación (theta): 100% del IMU si está disponible.
+                      - Traslación (x, y): 100% de los ticks de encoders siempre.
+        """
         self.METROS_POR_TICK = (2.0 * math.pi * self.RADIO_RUEDA) / self.TICKS_POR_VUELTA
 
         try:
@@ -39,10 +53,17 @@ class ChasisESP32:
             print(f"❌ Error UART: {e}")
             self.puerto = None
 
+        # Referencia al IMU (puede ser None → se usa odometría de ruedas para theta)
+        self._imu = imu
+
         self.x, self.y, self.theta = 0.0, 0.0, 0.0
         self.ticks_anteriores = [0, 0, 0, 0]
         self.primera_lectura  = True
 
+        # Velocidad lineal estimada en m/s (calculada con Δt real entre tramas)
+        self._velocidad_ms = 0.0
+        self._t_anterior   = None   # tiempo (perf_counter) de la última trama T:
+
     # ── Velocidad máxima (promedio por lado) ───────────────────────────────────
     @property
     def vel_max_izq(self) -> float:
@@ -78,26 +99,44 @@ class ChasisESP32:
             except ValueError:
                 return False
 
+            import time as _time
             if self.primera_lectura:
                 self.ticks_anteriores = ticks_actuales
                 self.primera_lectura  = False
+                self._t_anterior      = _time.perf_counter()
                 return True
 
+            ahora = _time.perf_counter()
+            dt    = ahora - self._t_anterior
+            self._t_anterior = ahora
+
             delta = [ticks_actuales[i] - self.ticks_anteriores[i] for i in range(4)]
             self.ticks_anteriores = ticks_actuales
 
-            # Desplazamiento lineal de cada lado (metros)
+            # ── Traslación: 100% encoders ─────────────────────────────────────
-            # M1(FL) y M3(RL) → izquierda
+            # Cada lado promedia sus dos motores (delantero + trasero)
-            # M2(FR) y M4(RR) → derecha
+            # M1(FL) y M3(RL) → izquierda  |  M2(FR) y M4(RR) → derecha
             d_izq    = ((delta[0] + delta[2]) / 2.0) * self.METROS_POR_TICK
             d_der    = ((delta[1] + delta[3]) / 2.0) * self.METROS_POR_TICK
-
             d_centro = (d_izq + d_der) / 2.0
-            d_theta  = (d_der - d_izq) / self.ANCHO_EJE
 
-            self.x     += d_centro * math.cos(self.theta + d_theta / 2.0)
+            # Velocidad lineal (m/s) con Δt real entre tramas
-            self.y     += d_centro * math.sin(self.theta + d_theta / 2.0)
+            if dt > 0:
-            self.theta += d_theta
+                self._velocidad_ms = d_centro / dt
+
+            # ── Rotación: 100% IMU (o diferencial de encoders si no hay IMU) ──
+            if self._imu is not None:
+                theta_nuevo = self._imu.theta_rad
+            else:
+                d_theta     = (d_der - d_izq) / self.ANCHO_EJE
+                theta_nuevo = self.theta + d_theta
+
+            # ── Posición: integrar d_centro en la dirección del IMU ───────────
+            # Se usa el promedio de theta anterior y nuevo para mayor precisión
+            theta_medio = (self.theta + theta_nuevo) / 2.0
+            self.x     += d_centro * math.cos(theta_medio)
+            self.y     += d_centro * math.sin(theta_medio)
+            self.theta  = theta_nuevo
             return True
 
         # ── K: VEL_MAX calibrada por el ESP32 ─────────────────────────────────
@@ -125,6 +164,20 @@ class ChasisESP32:
 
         return False
 
+    # ── Propiedades de diagnóstico ─────────────────────────────────────────────
+
+    @property
+    def velocidad_ms(self) -> float:
+        """Velocidad lineal actual en m/s (calculada desde los ticks de encoders)."""
+        return self._velocidad_ms
+
+    @property
+    def theta_imu_deg(self) -> float:
+        """Ángulo del IMU en grados (nan si no hay IMU conectado)."""
+        if self._imu is None:
+            return float('nan')
+        return math.degrees(self._imu.theta_rad)
+
     # ── Envío de velocidad diferencial ─────────────────────────────────────────
     def enviar_velocidad(self, v: float, w: float) -> None:
         """