From 872b9e48d3fe1a2064e9757f10a36b3621e7674e Mon Sep 17 00:00:00 2001 From: Juan Date: Thu, 21 May 2026 11:34:08 -0500 Subject: [PATCH] Puesto el IMU funcionando bien y con distancias --- main.py | 191 ++++++++++++++++++++++++++++++++++++++++-------- modulo_esp32.py | 80 +++++++++++++++----- 2 files changed, 219 insertions(+), 52 deletions(-) diff --git a/main.py b/main.py index bf6c913..1a24330 100644 --- a/main.py +++ b/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 - trayectoria.append((px_c, py_c, robot.velocidad_ms)) + # Guardar trayectoria (solo posición, sin velocidad) + trayectoria.append((px_c, py_c)) - # ── Componer frame ───────────────────────────────────────────────── + # ── 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) + # ── 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) - # ── 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) - # Cuerpo amarillo-naranja + 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) - # 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: - v, w = cmd_a_velocidad(cmd) - robot.enviar_velocidad(v, w) - print(f"\r🤖 [{cmd}] — {estado['razon'][:55]:<55}", end="") + + 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 + 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: - # Detener mientras calibra para no interferir 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'): diff --git a/modulo_esp32.py b/modulo_esp32.py index 73ccc70..77441f0 100644 --- a/modulo_esp32.py +++ b/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,30 +122,50 @@ class ChasisESP32: delta = [ticks_actuales[i] - self.ticks_anteriores[i] for i in range(4)] self.ticks_anteriores = ticks_actuales - # ── Traslación: 100% encoders ───────────────────────────────────── - # Cada lado promedia sus dos motores (delantero + trasero) + # ── ZUPT (Zero Velocity Update) ────────────────────────────────────── + # 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 - if dt > 0: - self._velocidad_ms = d_centro / dt + # ── Detectar fase: GIRANDO vs AVANZANDO ────────────────────────────── + d_magnitud = abs(d_izq) + abs(d_der) + 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 ─────────── - # 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 + # ── Traslación y velocidad: SOLO en fase AVANZANDO ────────────────── + 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 # ── K: VEL_MAX calibrada por el ESP32 ───────────────────────────────── @@ -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)