drive_vf.py

# controllers/drive_Vf/drive_controller.py

from controller import Robot
from lector_vf import get_robot_attributes
import math

# --- Parámetros de simulación y físicos ---
TIMESTEP     = 64       # ms
WHEEL_RADIUS = 0.0205   # m
WHEEL_BASE   = 0.053    # distancia entre ruedas

# --- Funciones de tarea (sin cambios) ---

def patrol_simple(robot, r):
    ESTADO_AVANZAR = 0
    ESTADO_GIRAR   = 1
    
  
    left_motor  = robot.getDevice('left wheel motor')
    right_motor = robot.getDevice('right wheel motor')
    left_motor.setPosition(float('inf'))
    right_motor.setPosition(float('inf'))

    PATROL_TIME      = r["patrol_time"]
    MOVE_DISTANCE    = r["move_distance"]
    patrol_speed_obj = r["patrol_speed"]
    max_speed        = r["max_speed"]
    consumption      = r["consumption"]
    battery_initial  = r["battery_charge"]
    
    print(f"Configuración de patrulla: Tiempo = {PATROL_TIME} s; rango patrulla = {MOVE_DISTANCE} m")


    # velocidad lineal de vueltas → rad/s
    if patrol_speed_obj <= max_speed:
        v_lin = patrol_speed_obj / WHEEL_RADIUS
    else:
        v_lin = max_speed / WHEEL_RADIUS

    turning_speed = min(v_lin * 0.5, 0.8)
    move_time     = MOVE_DISTANCE / patrol_speed_obj

    start_time   = robot.getTime()
    estado       = ESTADO_AVANZAR
    last_change  = start_time
    girando      = False
    giro_derecha = True
    ciclos       = 0

    rotation_angle = math.pi
    rotation_time  = rotation_angle * (WHEEL_BASE / (2 * turning_speed * WHEEL_RADIUS)) * 1.1

    while robot.step(TIMESTEP) != -1:
        t_now = robot.getTime()
        elapsed_since = t_now - last_change

        # Comprueba tiempo total
        if t_now - start_time > PATROL_TIME:
            print("Patrulla completada.")
            break

        if estado == ESTADO_AVANZAR:
            if elapsed_since <= move_time:
                left_motor.setVelocity(v_lin)
                right_motor.setVelocity(v_lin)
            else:
                left_motor.setVelocity(0)
                right_motor.setVelocity(0)
                robot.step(TIMESTEP * 5)
                estado      = ESTADO_GIRAR
                last_change = robot.getTime()
                girando     = True

        else:  # ESTADO_GIRAR
            if girando and elapsed_since <= rotation_time:
                if giro_derecha:
                    left_motor.setVelocity(-turning_speed)
                    right_motor.setVelocity(turning_speed)
                else:
                    left_motor.setVelocity(turning_speed)
                    right_motor.setVelocity(-turning_speed)
            else:
                left_motor.setVelocity(0)
                right_motor.setVelocity(0)
                robot.step(TIMESTEP * 5)
                estado        = ESTADO_AVANZAR
                girando       = False
                giro_derecha  = not giro_derecha
                last_change   = robot.getTime()
                if not giro_derecha:
                    ciclos += 1

    left_motor.setVelocity(0)
    right_motor.setVelocity(0)
    print("Patrulla finalizada.")

def navigate_to_point(robot, r):
    left_motor  = robot.getDevice('left wheel motor')
    right_motor = robot.getDevice('right wheel motor')
    left_motor.setPosition(float('inf'))
    right_motor.setPosition(float('inf'))

    # Activa sensores de proximidad
    sensors = []
    for i in [0,1,2,5,6,7]:
        ps = robot.getDevice(f'ps{i}')
        ps.enable(TIMESTEP)
        sensors.append(ps)

    xpos            = r["xpos"]
    ypos            = r["ypos"]
    theta           = r["angle"]
    target_x        = r["target_x"]
    target_y        = r["target_y"]
    max_speed       = r["max_speed"]
    consumption     = r["consumption"]
    battery_initial = r["battery_charge"]
    nav_speed_obj   = r["navigation_speed"]

    lin_speed = min(nav_speed_obj, max_speed) / WHEEL_RADIUS

    start_time = robot.getTime()
    current_x  = xpos
    current_y  = ypos

    print(f"→ Navegación a ({target_x:.2f},{target_y:.2f}) a {nav_speed_obj:.2f} m/s")

    while robot.step(TIMESTEP) != -1:
        t_now    = robot.getTime()
        elapsed  = (t_now - start_time) / 60.0
        battery  = battery_initial - consumption * elapsed

        # Comprueba llegada
        dx = target_x - current_x
        dy = target_y - current_y
        dist = math.hypot(dx, dy)
        if dist < 0.005:
            print("Destino alcanzado.")
            break

        # Evitación simple
        obs_front = sensors[0].getValue() > 100 or sensors[5].getValue() > 100
        obs_left  = sensors[3].getValue() > 80 or sensors[4].getValue() > 80
        obs_right = sensors[1].getValue() > 80 or sensors[2].getValue() > 80

        if obs_front:
            left_motor.setVelocity(-0.3 * lin_speed)
            right_motor.setVelocity( 0.3 * lin_speed)
        elif obs_left:
            left_motor.setVelocity( 0.5 * lin_speed)
            right_motor.setVelocity( 0.2 * lin_speed)
        elif obs_right:
            left_motor.setVelocity( 0.2 * lin_speed)
            right_motor.setVelocity( 0.5 * lin_speed)
        else:
            desired_ang = math.atan2(dy, dx)
            err_ang     = desired_ang - theta
            err_ang     = math.atan2(math.sin(err_ang), math.cos(err_ang))
            if abs(err_ang) > 0.1:
                if err_ang > 0:
                    left_motor.setVelocity(-0.1 * lin_speed)
                    right_motor.setVelocity( 0.1 * lin_speed)
                else:
                    left_motor.setVelocity( 0.1 * lin_speed)
                    right_motor.setVelocity(-0.1 * lin_speed)
            else:
                left_motor.setVelocity(lin_speed)
                right_motor.setVelocity(lin_speed)

        # Odometría simulada
        v_l = left_motor.getVelocity()
        v_r = right_motor.getVelocity()
        dt  = TIMESTEP / 1000.0
        v   = (v_r + v_l) * 0.5 * WHEEL_RADIUS
        w   = (v_r - v_l) * WHEEL_RADIUS / WHEEL_BASE
        theta    += w * dt
        theta     = math.atan2(math.sin(theta), math.cos(theta))
        current_x += v * math.cos(theta) * dt
        current_y += v * math.sin(theta) * dt

        print(f"Distancia {dist:.3f} m; pos ≈ ({current_x:.3f},{current_y:.3f})")

    left_motor.setVelocity(0)
    right_motor.setVelocity(0)
    print("Navegación finalizada.")

def stop_robot(robot):
    """Detiene los dos motores y sale del bucle."""
    left_motor  = robot.getDevice('left wheel motor')
    right_motor = robot.getDevice('right wheel motor')
    left_motor.setPosition(float('inf'))
    right_motor.setPosition(float('inf'))
    left_motor.setVelocity(0)
    right_motor.setVelocity(0)
    while robot.step(TIMESTEP) != -1:
        pass

def main():
    # 1) Obtener atributos con razonamiento HermiT
    robots = get_robot_attributes()

    # 2) Iniciar controlador Webots
    robot = Robot()
    my_name = robot.getName()
    print(f"Robot controlador: {my_name}")

    # 3) Encontrar atributos para este robot
    num = int(''.join(filter(str.isdigit, my_name)))
    r   = next((x for x in robots if x['id'] == num), None)
    if not r:
        print(f"[ERROR] No se encontraron atributos para {my_name}")
        stop_robot(robot)
        return

    print(f"ID:{r['id']}  Batería:{r['battery_charge']:.1f}%  Behavior:{r['behavior']}")

    # 4) Gestión de batería crítica
    batt = r['battery_charge']
    if batt is not None and batt < 20.0:
        print(f"{my_name} → BATERÍA CRÍTICA ({batt:.1f}%) → MODO RECOVERY")
        stop_robot(robot)
        return

    # 5) Despacho según behavior inferido
    beh = r['behavior']
    if beh == "patrolling":
        patrol_simple(robot, r)
    elif beh == "navigating":
        navigate_to_point(robot, r)
    else:
        print(f"{my_name} → Comportamiento '{beh}' desconocido. Detenido.")
        stop_robot(robot)

if __name__ == "__main__":
    main()