Juan/RoboticaPython
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Puesto el IMU funcionando bien y con distancias

Browse Source
This commit is contained in:
Juan
2026-05-21 11:34:08 -05:00
parent 3770430173
commit 872b9e48d3
2 changed files with 219 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files
main.py
+159 -32
View File
@@ -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'):