285 lines
12 KiB
Python
285 lines
12 KiB
Python
import serial
|
||
import math
|
||
import logging
|
||
|
||
logger = logging.getLogger(__name__)
|
||
|
||
|
||
|
||
|
||
class ChasisESP32:
|
||
"""
|
||
Interfaz UART con el ESP32.
|
||
|
||
Protocolo de TX (RPi → ESP32):
|
||
M:s1,f1,s2,f2,s3,f3,s4,f4\\n
|
||
s = signo → -1 | 0 | 1
|
||
f = fracción de VEL_MAX → 0.000 – 1.000
|
||
S:\\n → Stop inmediato
|
||
C:\\n → Iniciar autocalibración
|
||
|
||
Protocolo de RX (ESP32 → RPi):
|
||
T:t1,t2,t3,t4\\n → Odometría acumulada (ticks con signo)
|
||
K:v1,v2,v3,v4\\n → VEL_MAX_TICKS calibrada por motor (ticks/s)
|
||
D:...\\n → Mensajes de debug del ESP32
|
||
"""
|
||
|
||
# ── Geometría ──────────────────────────────────────────────────────────────
|
||
TICKS_POR_VUELTA = 20.0
|
||
RADIO_RUEDA = 0.03 # metros
|
||
ANCHO_EJE = 0.11 # metros (distancia entre ruedas del mismo eje)
|
||
|
||
# VEL_MAX_TICKS se actualiza cuando el ESP32 envía la trama K:
|
||
# Valor inicial conservador: 200 ticks/s
|
||
# 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):
|
||
"""
|
||
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:
|
||
self.puerto = serial.Serial(puerto, baudrate, timeout=0.05)
|
||
print(f"✅ Conexión UART activa en {puerto}")
|
||
except Exception as e:
|
||
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:
|
||
|
||
# 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:
|
||
"""Ticks/s máximos del lado izquierdo (promedio M1 y M3)."""
|
||
return (self._VEL_MAX_TICKS[0] + self._VEL_MAX_TICKS[2]) / 2.0
|
||
|
||
@property
|
||
def vel_max_der(self) -> float:
|
||
"""Ticks/s máximos del lado derecho (promedio M2 y M4)."""
|
||
return (self._VEL_MAX_TICKS[1] + self._VEL_MAX_TICKS[3]) / 2.0
|
||
|
||
# ── Lectura de odometría y tramas de control ───────────────────────────────
|
||
def leer_odometria(self) -> bool:
|
||
"""
|
||
Lee una línea del ESP32 y la procesa.
|
||
Retorna True si se actualizó la odometría.
|
||
"""
|
||
if not (self.puerto and self.puerto.in_waiting > 0):
|
||
return False
|
||
|
||
try:
|
||
linea = self.puerto.readline().decode('utf-8', errors='ignore').strip()
|
||
except Exception:
|
||
return False
|
||
|
||
# ── T: odometría ──────────────────────────────────────────────────────
|
||
if linea.startswith("T:"):
|
||
partes = linea[2:].split(',')
|
||
if len(partes) != 4:
|
||
return False
|
||
try:
|
||
ticks_actuales = [int(p) for p in partes]
|
||
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
|
||
|
||
# ── 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
|
||
|
||
# ── 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: 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
|
||
|
||
# ── 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 ─────────────────────────────────
|
||
elif linea.startswith("K:"):
|
||
partes = linea[2:].split(',')
|
||
if len(partes) == 4:
|
||
try:
|
||
nuevos = [float(p) for p in partes]
|
||
# Validar que los valores sean razonables (10–2000 ticks/s)
|
||
if all(10.0 < v < 2000.0 for v in nuevos):
|
||
self._VEL_MAX_TICKS = nuevos
|
||
print(
|
||
f"\n✅ VEL_MAX actualizada → "
|
||
f"M1={nuevos[0]:.1f} M2={nuevos[1]:.1f} "
|
||
f"M3={nuevos[2]:.1f} M4={nuevos[3]:.1f} ticks/s"
|
||
)
|
||
else:
|
||
print(f"\n⚠️ VEL_MAX recibida fuera de rango: {nuevos} — ignorada")
|
||
except ValueError:
|
||
pass
|
||
|
||
# ── D: debug del ESP32 ────────────────────────────────────────────────
|
||
elif linea.startswith("D:"):
|
||
print(f"\r[ESP32] {linea} ", end="")
|
||
|
||
return False
|
||
|
||
# ── 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 (0 durante giros)."""
|
||
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:
|
||
"""
|
||
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)
|
||
"""
|
||
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)
|
||
|
||
# Convertir a ticks/s
|
||
v_izq_ticks = v_izq_ms / self.METROS_POR_TICK
|
||
v_der_ticks = v_der_ms / self.METROS_POR_TICK
|
||
|
||
# Velocidades por motor (izq = M1, M3 / der = M2, M4)
|
||
vel_motores = [v_izq_ticks, v_der_ticks, v_izq_ticks, v_der_ticks]
|
||
|
||
# Umbral mínimo de movimiento: 0.5 ticks/s ≈ movimiento nulo
|
||
UMBRAL = 0.5
|
||
|
||
partes = []
|
||
todos_cero = True
|
||
for i, vel in enumerate(vel_motores):
|
||
if abs(vel) < UMBRAL:
|
||
s, f = 0, 0.0
|
||
else:
|
||
s = 1 if vel > 0 else -1
|
||
# Normalizar respecto a la VEL_MAX de cada motor específico
|
||
f = min(abs(vel) / self._VEL_MAX_TICKS[i], 1.0)
|
||
todos_cero = False
|
||
partes.append(f"{s},{f:.3f}")
|
||
|
||
if todos_cero:
|
||
cmd = "S:\n"
|
||
else:
|
||
cmd = "M:" + ",".join(partes) + "\n"
|
||
|
||
print(f"\r[UART TX] → {cmd.strip():<55}", end="", flush=True)
|
||
try:
|
||
self.puerto.write(cmd.encode('utf-8'))
|
||
except Exception as e:
|
||
print(f"\n❌ Error al escribir UART: {e}")
|
||
|
||
# ── Calibración ───────────────────────────────────────────────────────────
|
||
def calibrar_pid(self) -> None:
|
||
"""Solicita autocalibración al ESP32. La respuesta K: actualizará VEL_MAX."""
|
||
if self.puerto:
|
||
try:
|
||
self.puerto.write(b"C:\n")
|
||
print("\n⚙️ Solicitud de calibración enviada al ESP32.")
|
||
except Exception as e:
|
||
print(f"\n❌ Error al enviar calibración: {e}") |