75 lines
2.8 KiB
Python
75 lines
2.8 KiB
Python
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
|