Imu y velocidad funcionan a medias

modulo_esp32.py

@@ -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:
         """