Puesto el IMU funcionando bien y con distancias
This commit is contained in:
+60
-20
@@ -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)
|
||||
|
||||
Reference in New Issue
Block a user