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()