From 37704301738927711a8abd1d139c764d8d64ef9f Mon Sep 17 00:00:00 2001 From: Juan Date: Thu, 21 May 2026 10:27:51 -0500 Subject: [PATCH] Imu y velocidad funcionan a medias --- main.py | 165 +++++++++++++++++++++++++++++++++++------------- modulo_esp32.py | 75 ++++++++++++++++++---- 2 files changed, 184 insertions(+), 56 deletions(-) diff --git a/main.py b/main.py index 7098e48..bf6c913 100644 --- a/main.py +++ b/main.py @@ -6,6 +6,7 @@ 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 @@ -13,12 +14,13 @@ 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 +MAPA_W, MAPA_H = 700, 700 +ESCALA = 150 # píxeles por metro ORIGEN = (MAPA_W // 2, MAPA_H // 2) # ── Objetos globales ─────────────────────────────────────────────────────────── -robot = ChasisESP32(puerto='/dev/ttyAMA0') +imu = IMU(bus=1) # MPU6050 vía I2C +robot = ChasisESP32(puerto='/dev/ttyAMA0', imu=imu) camara = CamaraD415() evasor = EvasorObstaculos() ejecutando = True @@ -26,8 +28,47 @@ 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) +# 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 ────────────────────────────────────────────────────────── @@ -55,52 +96,84 @@ def cmd_a_velocidad(cmd: str) -> tuple[float, float]: return tabla.get(cmd, (0.0, 0.0)) -# ── Dibujar mapa 2D ─────────────────────────────────────────────────────────── +# ── Dibujar mapa 2D ────────────────────────────────────────────────────────────── def dibujar_mapa(x: float, y: float, theta: float) -> np.ndarray: - global mapa_fondo + 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(0, min(MAPA_W - 1, px)) - py_c = max(0, min(MAPA_H - 1, py)) + px_c = max(1, min(MAPA_W - 2, px)) + py_c = max(1, min(MAPA_H - 2, py)) - cv2.circle(mapa_fondo, (px_c, py_c), 2, (80, 80, 80), -1) - mapa = mapa_fondo.copy() + # Guardar punto de trayectoria con velocidad actual + trayectoria.append((px_c, py_c, robot.velocidad_ms)) - # 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) + # ── Componer frame ───────────────────────────────────────────────── + mapa = _mapa_base.copy() - # 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) + # 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) - # Banner de calibración en curso + # ── 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, 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) + 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 del evasor - evasor.dibujar_estado(mapa) + # ── 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) - # 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") + # 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, - (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) + (10, panel_y + 56), cv2.FONT_HERSHEY_SIMPLEX, 0.38, + (140, 160, 140), 1, cv2.LINE_AA) return mapa @@ -129,6 +202,11 @@ def main(): 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() @@ -137,12 +215,8 @@ def main(): 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) + # 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) @@ -199,6 +273,7 @@ def main(): 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.") diff --git a/modulo_esp32.py b/modulo_esp32.py index f0c6231..73ccc70 100644 --- a/modulo_esp32.py +++ b/modulo_esp32.py @@ -1,5 +1,10 @@ import serial import math +import logging + +logger = logging.getLogger(__name__) + + class ChasisESP32: @@ -29,7 +34,16 @@ class ChasisESP32: # M1=FL, M2=FR, M3=RL, M4=RR _VEL_MAX_TICKS = [200.0, 200.0, 200.0, 200.0] - def __init__(self, puerto: str = '/dev/ttyAMA0', baudrate: int = 921600): + def __init__(self, puerto: str = '/dev/ttyAMA0', baudrate: int = 921600, + imu=None): + """ + Args: + puerto: Puerto serie del ESP32. + baudrate: Velocidad de comunicación. + imu: Instancia de modulo_imu.IMU ya iniciada, o None. + - Rotación (theta): 100% del IMU si está disponible. + - Traslación (x, y): 100% de los ticks de encoders siempre. + """ self.METROS_POR_TICK = (2.0 * math.pi * self.RADIO_RUEDA) / self.TICKS_POR_VUELTA try: @@ -39,10 +53,17 @@ class ChasisESP32: print(f"❌ Error UART: {e}") self.puerto = None + # Referencia al IMU (puede ser None → se usa odometría de ruedas para theta) + self._imu = imu + self.x, self.y, self.theta = 0.0, 0.0, 0.0 self.ticks_anteriores = [0, 0, 0, 0] self.primera_lectura = True + # Velocidad lineal estimada en m/s (calculada con Δt real entre tramas) + self._velocidad_ms = 0.0 + self._t_anterior = None # tiempo (perf_counter) de la última trama T: + # ── Velocidad máxima (promedio por lado) ─────────────────────────────────── @property def vel_max_izq(self) -> float: @@ -78,26 +99,44 @@ class ChasisESP32: except ValueError: return False + import time as _time if self.primera_lectura: self.ticks_anteriores = ticks_actuales self.primera_lectura = False + self._t_anterior = _time.perf_counter() return True + ahora = _time.perf_counter() + dt = ahora - self._t_anterior + self._t_anterior = ahora + delta = [ticks_actuales[i] - self.ticks_anteriores[i] for i in range(4)] self.ticks_anteriores = ticks_actuales - # Desplazamiento lineal de cada lado (metros) - # M1(FL) y M3(RL) → izquierda - # M2(FR) y M4(RR) → derecha - d_izq = ((delta[0] + delta[2]) / 2.0) * self.METROS_POR_TICK - d_der = ((delta[1] + delta[3]) / 2.0) * self.METROS_POR_TICK - + # ── Traslación: 100% encoders ───────────────────────────────────── + # Cada lado promedia sus dos motores (delantero + trasero) + # M1(FL) y M3(RL) → izquierda | M2(FR) y M4(RR) → derecha + d_izq = ((delta[0] + delta[2]) / 2.0) * self.METROS_POR_TICK + d_der = ((delta[1] + delta[3]) / 2.0) * self.METROS_POR_TICK d_centro = (d_izq + d_der) / 2.0 - d_theta = (d_der - d_izq) / self.ANCHO_EJE - self.x += d_centro * math.cos(self.theta + d_theta / 2.0) - self.y += d_centro * math.sin(self.theta + d_theta / 2.0) - self.theta += d_theta + # Velocidad lineal (m/s) con Δt real entre tramas + if dt > 0: + self._velocidad_ms = d_centro / dt + + # ── Rotación: 100% IMU (o diferencial de encoders si no hay IMU) ── + if self._imu is not None: + theta_nuevo = self._imu.theta_rad + else: + d_theta = (d_der - d_izq) / self.ANCHO_EJE + theta_nuevo = self.theta + d_theta + + # ── Posición: integrar d_centro en la dirección del IMU ─────────── + # Se usa el promedio de theta anterior y nuevo para mayor precisión + theta_medio = (self.theta + theta_nuevo) / 2.0 + self.x += d_centro * math.cos(theta_medio) + self.y += d_centro * math.sin(theta_medio) + self.theta = theta_nuevo return True # ── K: VEL_MAX calibrada por el ESP32 ───────────────────────────────── @@ -125,6 +164,20 @@ class ChasisESP32: return False + # ── Propiedades de diagnóstico ───────────────────────────────────────────── + + @property + def velocidad_ms(self) -> float: + """Velocidad lineal actual en m/s (calculada desde los ticks de encoders).""" + return self._velocidad_ms + + @property + def theta_imu_deg(self) -> float: + """Ángulo del IMU en grados (nan si no hay IMU conectado).""" + if self._imu is None: + return float('nan') + return math.degrees(self._imu.theta_rad) + # ── Envío de velocidad diferencial ───────────────────────────────────────── def enviar_velocidad(self, v: float, w: float) -> None: """