Puesto el IMU funcionando bien y con distancias

main.py

@@ -97,49 +97,99 @@ def cmd_a_velocidad(cmd: str) -> tuple[float, float]:
 
 
 # ── Dibujar mapa 2D ──────────────────────────────────────────────────────────────
-def dibujar_mapa(x: float, y: float, theta: float) -> np.ndarray:
+# 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
 
-    # Construir fondo solo la primera vez
     if _mapa_base is None:
         _mapa_base = _construir_base()
 
-    # Posición en píxeles
+    # 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 punto de trayectoria con velocidad actual
+    # Guardar trayectoria (solo posición, sin velocidad)
-    trayectoria.append((px_c, py_c, robot.velocidad_ms))
+    trayectoria.append((px_c, py_c))
 
-    # ── Componer frame ─────────────────────────────────────────────────
+    # ── Componer frame ─────────────────────────────────────────────────────
     mapa = _mapa_base.copy()
 
-    # Trayectoria con degradado temporal: gris viejo → cian brillante reciente
+    # ── Trayectoria del robot como línea continua ──────────────────────────
-    n = len(trayectoria)
+    if len(trayectoria) >= 2:
-    for idx, (tx, ty, vel) in enumerate(trayectoria):
+        pts = np.array(trayectoria, dtype=np.int32)
-        progreso = idx / max(n - 1, 1)          # 0.0 (antiguo) → 1.0 (reciente)
+        # Línea base (gris-azul)
-        # Color: de (50,50,80) gris-azul a (0,255,180) cian-verde
+        cv2.polylines(mapa, [pts.reshape(-1, 1, 2)], False, (60, 90, 120), 1)
-        b = int(80  + progreso * (255 - 80))
+        # Últimos 40 puntos en cian brillante para mostrar recorrido reciente
-        g = int(50  + progreso * (255 - 50))
+        reciente = pts[-40:].reshape(-1, 1, 2)
-        r = int(50  + progreso * (0   - 50))
+        cv2.polylines(mapa, [reciente], False, (0, 220, 160), 2)
-        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 ──────────────────────────────────────────────────────────────
+    # ── 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
-    # Sombra
+    cv2.circle(mapa, (px_c + 2, py_c + 2), radio + 2, (0, 0, 0), -1)   # 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 ─────────────────────────────────────────────────
+    # ── 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",
@@ -159,12 +209,14 @@ def dibujar_mapa(x: float, y: float, theta: float) -> np.ndarray:
                 (10, panel_y + 18), cv2.FONT_HERSHEY_SIMPLEX, 0.47,
                 (200, 220, 255), 1, cv2.LINE_AA)
 
-    # Fila 2: velocidad + IMU
+    # 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}°")
+               f"IMU={robot.theta_imu_deg:+.1f}°   [{modo_txt}]")
     cv2.putText(mapa, vel_txt,
                 (10, panel_y + 38), cv2.FONT_HERSHEY_SIMPLEX, 0.47,
-                (100, 255, 180), 1, cv2.LINE_AA)
+                modo_color, 1, cv2.LINE_AA)
 
     # Fila 3: vel_max motores + estado evasor
     ev = evasor.ultimo_estado
@@ -178,7 +230,20 @@ def dibujar_mapa(x: float, y: float, theta: float) -> np.ndarray:
     return mapa
 
 
-# ── Calibración asíncrona ─────────────────────────────────────────────────────
+# ── 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.
@@ -213,6 +278,11 @@ def main():
     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 (+/-)
+
     try:
         while True:
             # Cámara: solo se usa para el evasor de obstáculos, sin mostrar video
@@ -222,17 +292,70 @@ def main():
             estado = evasor.decidir_movimiento(escaner)
             cmd    = estado["comando"]
 
-            # Autopiloto — bloqueado durante calibración
+            # ── 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'][:55]:<55}", end="")
+                        print(f"\r🤖 [{cmd}] {estado['razon'][:50]:<50}", end="")
-            elif modo_autonomo and calibrando:
+
-                # Detener mientras calibra para no interferir
+                    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
+                        print(f"\n✓ Giro completado ({math.degrees(girado):.0f}°). Evaluando camino...")
+                    else:
+                        # Continuar girando
+                        robot.enviar_velocidad(0.0, w_giro_actual)
+                        print(f"\r🔄 Girando... {math.degrees(girado):.0f}° / 30°", 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)
+            mapa = dibujar_mapa(robot.x, robot.y, robot.theta, escaner)
             cv2.imshow("Mapa SLAM", mapa)
 
             # Teclado
@@ -243,11 +366,15 @@ def main():
                     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
                     modo_autonomo = True
 
             elif tecla == ord('m'):
                 print("\n🛑 MODO MANUAL")
                 modo_autonomo = False
+                estado_auto   = AUTO_LIBRE
                 robot.enviar_velocidad(0.0, 0.0)
 
             elif tecla == ord('c'):

modulo_esp32.py

@@ -34,6 +34,12 @@ class ChasisESP32:
     # M1=FL, M2=FR, M3=RL, M4=RR
     _VEL_MAX_TICKS = [200.0, 200.0, 200.0, 200.0]
 
+    # Umbral de asimetría de encoders para considerar que el robot está girando.
+    # ratio = |d_der - d_izq| / (|d_izq| + |d_der|)
+    # 0.0 = completamente recto, 1.0 = giro puro (ruedas opuestas)
+    # Si ratio > RATIO_GIRO_PURO → modo giro (no actualiza x, y, velocidad)
+    RATIO_GIRO_PURO = 0.55   # 55% de asimetría indica giro
+
     def __init__(self, puerto: str = '/dev/ttyAMA0', baudrate: int = 921600,
                  imu=None):
         """
@@ -64,6 +70,9 @@ class ChasisESP32:
         self._velocidad_ms = 0.0
         self._t_anterior   = None   # tiempo (perf_counter) de la última trama T:
 
+        # True cuando el robot está en fase de giro (encoders asimétricos)
+        self._en_giro = False
+
     # ── Velocidad máxima (promedio por lado) ───────────────────────────────────
     @property
     def vel_max_izq(self) -> float:
@@ -113,29 +122,49 @@ class ChasisESP32:
             delta = [ticks_actuales[i] - self.ticks_anteriores[i] for i in range(4)]
             self.ticks_anteriores = ticks_actuales
 
-            # ── Traslación: 100% encoders ─────────────────────────────────────
+            # ── ZUPT (Zero Velocity Update) ──────────────────────────────────────
-            # Cada lado promedia sus dos motores (delantero + trasero)
+            # Si TODOS los ticks son cero, el robot está completamente quieto.
+            # El IMU igual acumula drift (~1°/rato). Re-sincronizamos el ángulo
+            # del IMU al theta actual conocido para absorber ese drift.
+            # No se actualiza x, y ni theta.
+            if all(d == 0 for d in delta):
+                if self._imu is not None:
+                    self._imu.resetear_angulo(self.theta)
+                self._velocidad_ms = 0.0
+                self._en_giro      = False
+                return True
+
+            # Desplazamiento 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
             d_centro = (d_izq + d_der) / 2.0
 
-            # Velocidad lineal (m/s) con Δt real entre tramas
+            # ── Detectar fase: GIRANDO vs AVANZANDO ──────────────────────────────
-            if dt > 0:
+            d_magnitud = abs(d_izq) + abs(d_der)
-                self._velocidad_ms = d_centro / dt
+            if d_magnitud > 1e-6:
+                ratio = abs(d_der - d_izq) / d_magnitud
+                self._en_giro = ratio > self.RATIO_GIRO_PURO
+            else:
+                self._en_giro = False
 
-            # ── Rotación: 100% IMU (o diferencial de encoders si no hay IMU) ──
+            # ── Rotación: SIEMPRE 100% del 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 ───────────
+            # ── Traslación y velocidad: SOLO en fase AVANZANDO ──────────────────
-            # Se usa el promedio de theta anterior y nuevo para mayor precisión
+            if not self._en_giro:
+                if dt > 0:
+                    self._velocidad_ms = d_centro / dt
                 theta_medio = (self.theta + theta_nuevo) / 2.0
                 self.x     += d_centro * math.cos(theta_medio)
                 self.y     += d_centro * math.sin(theta_medio)
+            else:
+                self._velocidad_ms = 0.0
+
             self.theta = theta_nuevo
             return True
 
@@ -166,9 +195,14 @@ class ChasisESP32:
 
     # ── Propiedades de diagnóstico ─────────────────────────────────────────────
 
+    @property
+    def en_giro(self) -> bool:
+        """True cuando los encoders detectan que el robot está girando (no avanzando)."""
+        return self._en_giro
+
     @property
     def velocidad_ms(self) -> float:
-        """Velocidad lineal actual en m/s (calculada desde los ticks de encoders)."""
+        """Velocidad lineal actual en m/s (0 durante giros)."""
         return self._velocidad_ms
 
     @property
@@ -183,20 +217,26 @@ class ChasisESP32:
         """
         Convierte velocidad lineal (m/s) y angular (rad/s) al protocolo M:.
 
+        Movimiento por fases (GIRO luego AVANCE):
+          Si se reciben v y w ambos distintos de cero, se descarta v y se
+          ejecuta solo el giro. En el siguiente frame, cuando w=0, se avanza.
+          Esto garantiza que nunca se combinen rotación y traslación simultáneas.
+
         Cinemática diferencial:
             v_izq = v - w * (ANCHO_EJE / 2)
             v_der = v + w * (ANCHO_EJE / 2)
-
-        Cada rueda se envía con:
-            s = signo  (-1, 0, 1)
-            f = |vel_rueda_ticks| / VEL_MAX_TICKS del motor  (0.0 – 1.0)
-
-        Trama resultante:
-            M:s1,f1,s2,f2,s3,f3,s4,f4\\n
         """
         if not self.puerto:
             return
 
+        # ── Movimiento por fases: GIRO tiene prioridad sobre AVANCE ───────────
+        UMBRAL_V = 0.01   # m/s mínimo considerado "intención de avanzar"
+        UMBRAL_W = 0.05   # rad/s mínimo considerado "intención de girar"
+        if abs(w) > UMBRAL_W and abs(v) > UMBRAL_V:
+            # Ambos pedidos: ejecutar solo el giro, ignorar v este frame
+            logger.debug("Fase giro primero: v=%.3f ignorado (w=%.3f)", v, w)
+            v = 0.0
+
         # Velocidades en m/s para cada lado
         v_izq_ms = v - w * (self.ANCHO_EJE / 2.0)
         v_der_ms = v + w * (self.ANCHO_EJE / 2.0)