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

Imu y velocidad funcionan a medias

Browse Source
This commit is contained in:
Juan
2026-05-21 10:27:51 -05:00
parent 19dc4bcf90
commit 3770430173
2 changed files with 184 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files
main.py
+120 -45
View File
@@ -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.")
modulo_esp32.py
+64 -11
View File
@@ -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:
"""