Funciona todo sin modulo_imu

main.py

@@ -0,0 +1,208 @@
+import time
+import threading
+import numpy as np
+import cv2
+import math
+from modulo_esp32 import ChasisESP32
+from modulo_camara import CamaraD415
+from modulo_slam import EvasorObstaculos
+
+# ── Constantes de velocidad ────────────────────────────────────────────────────
+VEL_NORMAL = 0.50    # m/s en camino libre
+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
+ESCALA  = 200          # píxeles por metro
+ORIGEN  = (MAPA_W // 2, MAPA_H // 2)
+
+# ── Objetos globales ───────────────────────────────────────────────────────────
+robot      = ChasisESP32(puerto='/dev/ttyAMA0')
+camara     = CamaraD415()
+evasor     = EvasorObstaculos()
+ejecutando = True
+
+# Flag de calibración en curso (evita que el autopiloto interfiera)
+calibrando = False
+
+# ── Mapa acumulado ─────────────────────────────────────────────────────────────
+mapa_fondo = np.zeros((MAPA_H, MAPA_W, 3), dtype=np.uint8)
+
+
+# ── Hilo de hardware ──────────────────────────────────────────────────────────
+def hilo_hardware():
+    """
+    Lee odometría y tramas de control continuamente.
+    Incluye las tramas K: (VEL_MAX) que procesa modulo_esp32 internamente.
+    """
+    global ejecutando
+    while ejecutando:
+        robot.leer_odometria()
+        time.sleep(0.005)   # 5ms → ~200 lecturas/s, suficiente para 40ms de telemetría
+
+
+# ── Traducción CMD → (v, w) ────────────────────────────────────────────────────
+def cmd_a_velocidad(cmd: str) -> tuple[float, float]:
+    tabla = {
+        EvasorObstaculos.CMD_ADELANTE      : (-VEL_NORMAL, 0.0),
+        EvasorObstaculos.CMD_ADELANTE_LENTO: (-VEL_LENTA,  0.0),
+        EvasorObstaculos.CMD_GIRO_IZQ      : (0.0, -VEL_GIRO),
+        EvasorObstaculos.CMD_GIRO_DER      : (0.0,  VEL_GIRO),
+        EvasorObstaculos.CMD_RETROCEDER    : (VEL_NORMAL, 0.0),
+        EvasorObstaculos.CMD_DETENIDO      : (0.0,  0.0),
+    }
+    return tabla.get(cmd, (0.0, 0.0))
+
+
+# ── Dibujar mapa 2D ───────────────────────────────────────────────────────────
+def dibujar_mapa(x: float, y: float, theta: float) -> np.ndarray:
+    global mapa_fondo
+
+    px = int(ORIGEN[0] + x * ESCALA)
+    py = int(ORIGEN[1] - y * ESCALA)
+    px_c = max(0, min(MAPA_W - 1, px))
+    py_c = max(0, min(MAPA_H - 1, py))
+
+    cv2.circle(mapa_fondo, (px_c, py_c), 2, (80, 80, 80), -1)
+    mapa = mapa_fondo.copy()
+
+    # Cuadrícula (cada metro)
+    for i in range(0, MAPA_W, ESCALA):
+        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
+    radio = 10
+    cv2.circle(mapa, (px_c, py_c), radio, (0, 200, 255), 2)
+    fx = int(px_c + math.cos(theta) * radio * 1.6)
+    fy = int(py_c - math.sin(theta) * radio * 1.6)
+    cv2.arrowedLine(mapa, (px_c, py_c), (fx, fy), (0, 200, 255), 2, tipLength=0.4)
+
+    # Banner de calibración en curso
+    if calibrando:
+        cv2.rectangle(mapa, (0, 0), (MAPA_W, 28), (0, 60, 120), -1)
+        cv2.putText(mapa, "⚙ CALIBRANDO — espera la trama K:",
+                    (8, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.52, (0, 220, 255), 1)
+
+    # HUD del evasor
+    evasor.dibujar_estado(mapa)
+
+    # VEL_MAX actual (para diagnóstico visual)
+    vmax_txt = (f"VelMax  izq={robot.vel_max_izq:.0f}  "
+                f"der={robot.vel_max_der:.0f}  ticks/s")
+    cv2.putText(mapa, vmax_txt,
+                (8, MAPA_H - 24), cv2.FONT_HERSHEY_SIMPLEX, 0.38,
+                (80, 160, 80), 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
+
+
+# ── Calibración asíncrona ─────────────────────────────────────────────────────
+def lanzar_calibracion():
+    """
+    Ejecuta la calibración en un hilo aparte para no bloquear el bucle principal.
+    El ESP32 tarda ~2s en calibrar y responde con la trama K: automáticamente.
+    modulo_esp32.leer_odometria() la captura en el hilo de hardware.
+    """
+    global calibrando
+    calibrando = True
+    print("\n⚙️  Iniciando calibración (el ESP32 responderá con K: al terminar)...")
+    robot.calibrar_pid()
+    # Esperamos hasta 8s; si el ESP32 actualiza VEL_MAX antes, el flag
+    # se puede bajar manualmente, pero aquí simplemente esperamos el tiempo máximo.
+    time.sleep(8.0)
+    calibrando = False
+    print("\n✅ Tiempo de calibración agotado — VEL_MAX activa en el sistema.")
+
+
+# ── Main ───────────────────────────────────────────────────────────────────────
+def main():
+    global ejecutando
+    print("\n🚀 INICIANDO SISTEMA AUTÓNOMO...")
+    print("Teclas (ventana Mapa): [a] Auto | [m] Manual | [c] Calibrar | [q] Salir")
+
+    hilo = threading.Thread(target=hilo_hardware, daemon=True)
+    hilo.start()
+
+    modo_autonomo   = False
+    hilo_calib      = None
+
+    try:
+        while True:
+            escaner, frame_video = 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)
+            cmd    = estado["comando"]
+
+            # Autopiloto — bloqueado durante calibración
+            if modo_autonomo and not calibrando:
+                v, w = cmd_a_velocidad(cmd)
+                robot.enviar_velocidad(v, w)
+                print(f"\r🤖 [{cmd}] — {estado['razon'][:55]:<55}", end="")
+            elif modo_autonomo and calibrando:
+                # Detener mientras calibra para no interferir
+                robot.enviar_velocidad(0.0, 0.0)
+
+            # Mapa 2D
+            mapa = dibujar_mapa(robot.x, robot.y, robot.theta)
+            cv2.imshow("Mapa SLAM", mapa)
+
+            # Teclado
+            tecla = cv2.waitKey(1) & 0xFF
+
+            if tecla == ord('a'):
+                if calibrando:
+                    print("\n⚠️  Calibración en curso — espera antes de activar autopiloto.")
+                else:
+                    print("\n🤖 AUTOPILOTO ON")
+                    modo_autonomo = True
+
+            elif tecla == ord('m'):
+                print("\n🛑 MODO MANUAL")
+                modo_autonomo = False
+                robot.enviar_velocidad(0.0, 0.0)
+
+            elif tecla == ord('c'):
+                if calibrando:
+                    print("\n⚠️  Ya hay una calibración en curso.")
+                else:
+                    # Detener robot primero
+                    modo_autonomo = False
+                    robot.enviar_velocidad(0.0, 0.0)
+                    time.sleep(0.2)
+                    # Lanzar calibración en hilo para no bloquear la UI
+                    hilo_calib = threading.Thread(target=lanzar_calibracion, daemon=True)
+                    hilo_calib.start()
+
+            elif tecla == ord('q'):
+                break
+
+    except KeyboardInterrupt:
+        pass
+    finally:
+        ejecutando = False
+        hilo.join(timeout=1.0)
+        if hilo_calib and hilo_calib.is_alive():
+            hilo_calib.join(timeout=2.0)
+        robot.enviar_velocidad(0.0, 0.0)
+        camara.cerrar()
+        cv2.destroyAllWindows()
+        print("\n🔌 Apagado.")
+
+
+if __name__ == "__main__":
+    main()