432 lines
20 KiB
Python
432 lines
20 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.65 # m/s en camino libre
|
|
VEL_LENTA = 0.40 # m/s en precaución
|
|
VEL_GIRO = 2 # 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 ──────────────────────────────────────────────────────────────
|
|
# FOV horizontal de la RealSense D415 (profundidad) en radianes
|
|
CAMARA_FOV_RAD = math.radians(69)
|
|
|
|
|
|
def dibujar_mapa(x: float, y: float, theta: float,
|
|
escaner: np.ndarray | None = None) -> np.ndarray:
|
|
global _mapa_base
|
|
|
|
if _mapa_base is None:
|
|
_mapa_base = _construir_base()
|
|
|
|
# Posición del robot 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 trayectoria (solo posición, sin velocidad)
|
|
trayectoria.append((px_c, py_c))
|
|
|
|
# ── Componer frame ─────────────────────────────────────────────────────
|
|
mapa = _mapa_base.copy()
|
|
|
|
# ── Trayectoria del robot como línea continua ──────────────────────────
|
|
if len(trayectoria) >= 2:
|
|
pts = np.array(trayectoria, dtype=np.int32)
|
|
# Línea base (gris-azul)
|
|
cv2.polylines(mapa, [pts.reshape(-1, 1, 2)], False, (60, 90, 120), 1)
|
|
# Últimos 40 puntos en cian brillante para mostrar recorrido reciente
|
|
reciente = pts[-40:].reshape(-1, 1, 2)
|
|
cv2.polylines(mapa, [reciente], False, (0, 220, 160), 2)
|
|
|
|
# ── Campo de visión de la cámara (rayos 2D) ────────────────────────────
|
|
if escaner is not None and len(escaner) == 640:
|
|
N = 640
|
|
MAX_DIST_M = 4.5 # metros máximos a visualizar
|
|
PASO = 5 # dibujar 1 rayo cada 5 píxeles (128 rayos)
|
|
|
|
# Construir polígono del área visible (para rellenar con color)
|
|
pts_fov = [(px_c, py_c)]
|
|
for i in range(0, N, PASO):
|
|
d = float(escaner[i])
|
|
ang = theta + (0.5 - i / (N - 1.0)) * CAMARA_FOV_RAD
|
|
d_vis = min(d if 0.05 < d < MAX_DIST_M else MAX_DIST_M, MAX_DIST_M)
|
|
ex = int(px_c + math.cos(ang) * d_vis * ESCALA)
|
|
ey = int(py_c - math.sin(ang) * d_vis * ESCALA)
|
|
pts_fov.append((max(0, min(MAPA_W-1, ex)), max(0, min(MAPA_H-1, ey))))
|
|
pts_fov.append((px_c, py_c))
|
|
|
|
# Relleno semitransparente del área de visión (verde muy oscuro)
|
|
arr_fov = np.array(pts_fov, dtype=np.int32).reshape(-1, 1, 2)
|
|
overlay = mapa.copy()
|
|
cv2.fillPoly(overlay, [arr_fov], (0, 55, 25))
|
|
cv2.addWeighted(overlay, 0.30, mapa, 0.70, 0, mapa)
|
|
|
|
# Líneas de rayos individuales coloreadas por distancia
|
|
for i in range(0, N, PASO):
|
|
d = float(escaner[i])
|
|
if d < 0.05:
|
|
continue # lectura inválida
|
|
ang = theta + (0.5 - i / (N - 1.0)) * CAMARA_FOV_RAD
|
|
d_vis = min(d, MAX_DIST_M)
|
|
ex = int(px_c + math.cos(ang) * d_vis * ESCALA)
|
|
ey = int(py_c - math.sin(ang) * d_vis * ESCALA)
|
|
ex = max(0, min(MAPA_W - 1, ex))
|
|
ey = max(0, min(MAPA_H - 1, ey))
|
|
|
|
if d < evasor.dist_peligro:
|
|
color = (30, 40, 255) # rojo vivo — peligro
|
|
elif d < evasor.dist_precaucion:
|
|
color = (0, 165, 255) # naranja — precaución
|
|
elif d < evasor.dist_libre:
|
|
color = (0, 210, 120) # verde — libre
|
|
else:
|
|
color = (30, 100, 50) # verde oscuro — muy lejos
|
|
|
|
cv2.line(mapa, (px_c, py_c), (ex, ey), color, 1)
|
|
if d < evasor.dist_precaucion: # marcar hits cercanos
|
|
cv2.circle(mapa, (ex, ey), 3, color, -1)
|
|
|
|
# Contorno del FOV (borde blanco-azulado)
|
|
cv2.polylines(mapa, [arr_fov], True, (80, 130, 180), 1)
|
|
|
|
# ── Robot (encima de los rayos) ────────────────────────────────────────
|
|
radio = 12
|
|
cv2.circle(mapa, (px_c + 2, py_c + 2), radio + 2, (0, 0, 0), -1) # sombra
|
|
cv2.circle(mapa, (px_c, py_c), radio, (0, 200, 255), -1)
|
|
cv2.circle(mapa, (px_c, py_c), radio, (0, 255, 255), 2)
|
|
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 + modo de movimiento
|
|
modo_txt = "GIRANDO" if robot.en_giro else "AVANZANDO"
|
|
modo_color = (100, 160, 255) if robot.en_giro else (100, 255, 180)
|
|
vel_txt = (f"V={robot.velocidad_ms * 100:.1f} cm/s "
|
|
f"IMU={robot.theta_imu_deg:+.1f}° [{modo_txt}]")
|
|
cv2.putText(mapa, vel_txt,
|
|
(10, panel_y + 38), cv2.FONT_HERSHEY_SIMPLEX, 0.47,
|
|
modo_color, 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
|
|
|
|
|
|
# ── Autopiloto: máquina de estados para giros de 30° ───────────────────────────
|
|
AUTO_LIBRE = "LIBRE" # avanzando según el evasor
|
|
AUTO_GIRANDO = "GIRANDO" # ejecutando un paso de giro de 30°
|
|
AUTO_CHECK = "CHECK" # detenido, evaluando si el camino quedó libre
|
|
|
|
PASO_GIRO_RAD = math.radians(30) # tamaño del paso de giro
|
|
TOLERANCIA_RAD = math.radians(4) # margen de llegada al ángulo objetivo
|
|
|
|
|
|
def _diff_angulo(a: float, b: float) -> float:
|
|
"""Diferencia normalizada (a - b) en el rango (-π, π]."""
|
|
return (a - b + math.pi) % (2 * math.pi) - math.pi
|
|
|
|
|
|
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
|
|
|
|
# ── Estado del autopiloto (máquina de giros de 30°) ───────────────────────
|
|
estado_auto = AUTO_LIBRE # estado actual de la máquina
|
|
theta_inicio = 0.0 # theta del IMU al iniciar el paso de giro
|
|
w_giro_actual = 0.0 # dirección del giro en curso (+/-)
|
|
incremento_w = 0.0 # compensación dinámica de giro por fricción
|
|
ultimo_tiempo_auto = time.perf_counter()
|
|
|
|
try:
|
|
while True:
|
|
ahora = time.perf_counter()
|
|
dt = ahora - ultimo_tiempo_auto
|
|
ultimo_tiempo_auto = ahora
|
|
|
|
# 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: máquina de estados ────────────────────────────────────────
|
|
if modo_autonomo and not calibrando:
|
|
|
|
if estado_auto == AUTO_LIBRE:
|
|
# ── Camino despejado: seguir al evasor ─────────────────────
|
|
if cmd in (EvasorObstaculos.CMD_ADELANTE,
|
|
EvasorObstaculos.CMD_ADELANTE_LENTO,
|
|
EvasorObstaculos.CMD_RETROCEDER,
|
|
EvasorObstaculos.CMD_DETENIDO):
|
|
v, w = cmd_a_velocidad(cmd)
|
|
robot.enviar_velocidad(v, w)
|
|
print(f"\r🤖 [{cmd}] {estado['razon'][:50]:<50}", end="")
|
|
|
|
elif cmd in (EvasorObstaculos.CMD_GIRO_IZQ,
|
|
EvasorObstaculos.CMD_GIRO_DER):
|
|
# ── Obstáculo detectado: iniciar primer paso de 30° ──
|
|
_, w_giro_actual = cmd_a_velocidad(cmd)
|
|
theta_inicio = robot.theta
|
|
estado_auto = AUTO_GIRANDO
|
|
robot.enviar_velocidad(0.0, w_giro_actual)
|
|
print(f"\n🔄 Iniciando giro 30° (w={w_giro_actual:+.1f} rad/s)")
|
|
|
|
elif estado_auto == AUTO_GIRANDO:
|
|
# ── Girando: medir cuánto se ha rotado desde theta_inicio ──────
|
|
girado = abs(_diff_angulo(robot.theta, theta_inicio))
|
|
if girado >= PASO_GIRO_RAD - TOLERANCIA_RAD:
|
|
# Completó ~30° → detener y evaluar
|
|
robot.enviar_velocidad(0.0, 0.0)
|
|
estado_auto = AUTO_CHECK
|
|
incremento_w = 0.0
|
|
print(f"\n✓ Giro completado ({math.degrees(girado):.0f}°). Evaluando camino...")
|
|
else:
|
|
# ── Compensación de fricción ──
|
|
# Si el robot está atascado (IMU marca casi cero giro), aumentamos el PWM
|
|
w_real = abs(imu.omega_x_rads)
|
|
if w_real < 0.2:
|
|
incremento_w += 1.5 * dt # Rampa de subida (1.5 rad/s por segundo)
|
|
else:
|
|
incremento_w -= 3.0 * dt # Rampa de bajada más rápida si ya gira
|
|
|
|
# Limitar entre 0 y 2.5 rad/s extra para que no se vuelva loco
|
|
incremento_w = max(0.0, min(incremento_w, 2.5))
|
|
|
|
w_compensado = w_giro_actual + math.copysign(incremento_w, w_giro_actual)
|
|
|
|
# Continuar girando
|
|
robot.enviar_velocidad(0.0, w_compensado)
|
|
print(f"\r🔄 Girando... {math.degrees(girado):.0f}° / 30° (Boost: {incremento_w:.1f})", end="")
|
|
|
|
elif estado_auto == AUTO_CHECK:
|
|
# ── Verificar si el camino quedó libre ────────────────────
|
|
if cmd in (EvasorObstaculos.CMD_ADELANTE,
|
|
EvasorObstaculos.CMD_ADELANTE_LENTO):
|
|
# ¡Camino libre! Volver a avanzar
|
|
estado_auto = AUTO_LIBRE
|
|
v, w = cmd_a_velocidad(cmd)
|
|
robot.enviar_velocidad(v, w)
|
|
print(f"\n✅ Camino libre — avanzando")
|
|
|
|
elif cmd in (EvasorObstaculos.CMD_GIRO_IZQ,
|
|
EvasorObstaculos.CMD_GIRO_DER):
|
|
# Sigue bloqueado: otro paso de 30° en el mismo sentido
|
|
theta_inicio = robot.theta
|
|
estado_auto = AUTO_GIRANDO
|
|
robot.enviar_velocidad(0.0, w_giro_actual)
|
|
print(f"\n🔄 Sigue bloqueado — otro paso de 30°")
|
|
|
|
else: # RETROCEDER o DETENIDO
|
|
estado_auto = AUTO_LIBRE
|
|
v, w = cmd_a_velocidad(cmd)
|
|
robot.enviar_velocidad(v, w)
|
|
|
|
elif modo_autonomo and calibrando:
|
|
robot.enviar_velocidad(0.0, 0.0)
|
|
estado_auto = AUTO_LIBRE
|
|
|
|
# Mapa 2D
|
|
mapa = dibujar_mapa(robot.x, robot.y, robot.theta, escaner)
|
|
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")
|
|
estado_auto = AUTO_LIBRE
|
|
theta_inicio = robot.theta
|
|
w_giro_actual = 0.0
|
|
incremento_w = 0.0
|
|
modo_autonomo = True
|
|
|
|
elif tecla == ord('m'):
|
|
print("\n🛑 MODO MANUAL")
|
|
modo_autonomo = False
|
|
estado_auto = AUTO_LIBRE
|
|
incremento_w = 0.0
|
|
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() |