Colony

% For now I am ignoring ThetaD and just computing an arc

% This function expects to NOT have the quees be at index 1

function [V,W] = computeTrajectories(X,Y,Theta,XD,YD,ThetaD,XYError,ThetaError,LastV,LastW)

Kp = 1/7;

Ki = 1/100;

Kd = 1/30;

% how far away are we

dists = sqrt((X-XD).^2 + (Y-YD) .^ 2);

for idx=1:size(X,1)

    %V(idx) = dists(idx)/Kp; % linear velocity is proportional to distance

    %W(idx) = (ThetaD(idx) - Theta(idx))/Kp;

    curXYError = cos(Theta(idx))*(XD(idx) - X(idx)) + sin(Theta(idx))*(YD(idx) - Y(idx));

    curThetaError = (-sin(Theta(idx))*(XD(idx) - X(idx)) + cos(Theta(idx))*(YD(idx) - Y(idx)));

    XYError(idx) = XYError(idx) + curXYError;

    ThetaError(idx) = ThetaError(idx) + curThetaError;

    V(idx) = curXYError*Kp + XYError(idx)*Ki + LastV(idx)*Kd;

    W(idx) = curThetaError*Kp + ThetaError(idx)*Ki + LastW(idx)*Kd;

end

end