Funciona todo sin modulo_imu

modulo_esp32.py

@@ -0,0 +1,192 @@
+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}")