Untitled

                Never    
function [w,error] = tracker(xr,yr,thetar,xd,yd,Kw)
    %ENTRADAS:% xr e yr: coordenadas x e y obtenidas de la odometria.
    % thetar: orientacion del robot obtenida del robot.
    % xd e yd: coordenadas x e y de la posición del waypoint actual al que queremos llegar.
    % Kw: constante del bucle proporcional.
    %SALIDAS:
    % w: Velocidad angular necesaria para llegar al waypoint.
    % error: variable error para registrar en el logger.
    v=[0 0]; %vector que va desde la posición del robot (C en la figura) hasta la del waypoint.

    v(1) = xd-xr;
    v(2) = yd-yr;

    thetat=atan2(v(2),v(1)); % ángulo que forma ese vector con el eje X del sistema de referencias universal(ángulo ?T en la figura).

    if thetat < 0
       thetat = thetat + 2*pi;
    end %Normalizacion del angulo de orientacion del robot.

    if thetar >= 2*pi
       thetar = thetar - 2*pi;
    elseif thetar <=0
       thetar = thetar + 2*pi;
    end

    thetad = thetat-thetar; %Señal de error del bucle.

    if thetad < -pi
       thetad = thetad + 2*pi;
    elseif thetad > pi
       thetad = -(2*pi - thetad);
    end

    %Señales de salida:
    error = thetad;
    w = Kw * thetad;

    if w > 7
       w = 7;
    elseif w < -7
           w = -7;
    end
end

Raw Text