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}")