283 lines
12 KiB
Python
283 lines
12 KiB
Python
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() |