192 lines
7.9 KiB
Python
192 lines
7.9 KiB
Python
import serial
|
||
import math
|
||
|
||
|
||
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]
|
||
|
||
def __init__(self, puerto: str = '/dev/ttyAMA0', baudrate: int = 921600):
|
||
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
|
||
|
||
self.x, self.y, self.theta = 0.0, 0.0, 0.0
|
||
self.ticks_anteriores = [0, 0, 0, 0]
|
||
self.primera_lectura = True
|
||
|
||
# ── 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
|
||
|
||
if self.primera_lectura:
|
||
self.ticks_anteriores = ticks_actuales
|
||
self.primera_lectura = False
|
||
return True
|
||
|
||
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
|
||
|
||
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
|
||
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
|
||
|
||
# ── 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:.
|
||
|
||
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
|
||
|
||
# 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}") |