Funciona todo sin modulo_imu

seguidor_ruta.py

@@ -0,0 +1,74 @@
+import math
+from typing import Tuple, List
+
+class SeguidorRuta:
+    """
+    Controlador para que el robot siga una serie de puntos (ruta A*).
+    Utiliza un enfoque simplificado: gira hacia el waypoint y avanza.
+    """
+
+    def __init__(self, 
+                 umbral_distancia: float = 0.15,
+                 vel_max: float = 0.50,
+                 vel_giro: float = 1.50):
+        """
+        Args:
+            umbral_distancia: Distancia en metros a la que se considera alcanzado un waypoint.
+            vel_max: Velocidad máxima de avance (m/s).
+            vel_giro: Velocidad máxima de giro (rad/s).
+        """
+        self.umbral_distancia = umbral_distancia
+        self.vel_max = vel_max
+        self.vel_giro = vel_giro
+
+    def calcular_velocidad(self, 
+                           x_robot: float, 
+                           y_robot: float, 
+                           theta_robot: float, 
+                           ruta: List[Tuple[float, float]]) -> Tuple[float, float, List[Tuple[float, float]]]:
+        """
+        Calcula la velocidad lineal y angular para dirigirse al siguiente waypoint.
+        Retorna (v, w, ruta_actualizada).
+        """
+        if not ruta:
+            return 0.0, 0.0, []
+
+        wx, wy = ruta[0]
+        
+        # Calcular distancia al waypoint actual
+        distancia = math.hypot(wy - y_robot, wx - x_robot)
+        
+        # Si estamos lo suficientemente cerca, pasar al siguiente punto
+        if distancia < self.umbral_distancia:
+            ruta.pop(0)
+            if not ruta:
+                # Llegamos al destino final
+                return 0.0, 0.0, []
+            # Tomar las coordenadas del nuevo waypoint
+            wx, wy = ruta[0]
+
+        # Calcular ángulo hacia el waypoint respecto al marco global
+        angulo_objetivo = math.atan2(wy - y_robot, wx - x_robot)
+        
+        # Calcular error de ángulo (relativo al robot)
+        error_angulo = angulo_objetivo - theta_robot
+        
+        # Normalizar el error al rango [-pi, pi] para girar por el lado más corto
+        error_angulo = (error_angulo + math.pi) % (2 * math.pi) - math.pi
+
+        # Lógica de movimiento
+        # Si el error es grande (> 20 grados aprox), priorizar giro en el propio eje
+        umbral_giro = 0.35
+        
+        if abs(error_angulo) > umbral_giro:
+            v = 0.0
+            # Girar a la velocidad máxima en la dirección correspondiente
+            w = self.vel_giro if error_angulo > 0 else -self.vel_giro
+        else:
+            # Avanzar y corregir ligeramente la trayectoria (Control Proporcional suave)
+            v = self.vel_max
+            # Escala el giro suavemente pero sin superar el maximo
+            w = error_angulo * (self.vel_giro / umbral_giro) 
+            w = max(-self.vel_giro, min(self.vel_giro, w))
+
+        return v, w, ruta