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 from modulo_imu import IMU # ── 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 = 700, 700 ESCALA = 150 # píxeles por metro ORIGEN = (MAPA_W // 2, MAPA_H // 2) # ── Objetos globales ─────────────────────────────────────────────────────────── imu = IMU(bus=1) # MPU6050 vía I2C robot = ChasisESP32(puerto='/dev/ttyAMA0', imu=imu) camara = CamaraD415() evasor = EvasorObstaculos() ejecutando = True # Flag de calibración en curso (evita que el autopiloto interfiera) calibrando = False # Trayectoria acumulada: lista de (px, py, velocidad_ms) para colorear por velocidad 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 ────────────────────────────────────────────────────────── 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_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(1, min(MAPA_W - 2, px)) py_c = max(1, min(MAPA_H - 2, py)) # Guardar punto de trayectoria con velocidad actual trayectoria.append((px_c, py_c, robot.velocidad_ms)) # ── Componer frame ───────────────────────────────────────────────── mapa = _mapa_base.copy() # Trayectoria con degradado temporal: gris viejo → cian brillante reciente n = len(trayectoria) for idx, (tx, ty, vel) in enumerate(trayectoria): progreso = idx / max(n - 1, 1) # 0.0 (antiguo) → 1.0 (reciente) # Color: de (50,50,80) gris-azul a (0,255,180) cian-verde 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) # ── 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, 30), (0, 50, 110), -1) cv2.putText(mapa, " CALIBRANDO — manten robot en reposo", (8, 22), cv2.FONT_HERSHEY_SIMPLEX, 0.55, (0, 220, 255), 1) # ── HUD inferior ──────────────────────────────────────────────────────────── # 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) # Fila 1: pose del robot pose_txt = (f"x={x:+.2f}m y={y:+.2f}m θ={math.degrees(theta):+.1f}°") 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, (10, panel_y + 56), cv2.FONT_HERSHEY_SIMPLEX, 0.38, (140, 160, 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") # ── 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() modo_autonomo = False hilo_calib = None try: while True: # Cámara: solo se usa para el evasor de obstáculos, sin mostrar video escaner, _ = camara.obtener_datos() # 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) imu.detener() camara.cerrar() cv2.destroyAllWindows() print("\n🔌 Apagado.") if __name__ == "__main__": main()