Project

General

Profile

Statistics
| Revision:

root / branches / 16299_s10 / matlab / computeTrajectories.m @ 1812

History | View | Annotate | Download (583 Bytes)

1
% For now I am ignoring ThetaD and just computing an arc
2
% This function expects to NOT have the quees be at index 1
3
function [V,W] = computeTrajectories(X,Y,Theta,XD,YD,ThetaD)
4

    
5
Kp = 7;
6

    
7
% how far away are we
8
dists = sqrt((X-XD).^2 + (Y-YD) .^ 2);
9

    
10

    
11
for idx=1:size(X,1)
12
    %V(idx) = dists(idx)/Kp; % linear velocity is proportional to distance
13
    %W(idx) = (ThetaD(idx) - Theta(idx))/Kp;
14
    V(idx) = (cos(Theta(idx))*(XD(idx) - X(idx)) + sin(Theta(idx))*(YD(idx) - Y(idx)))/Kp;
15
    W(idx) = (-sin(Theta(idx))*(XD(idx) - X(idx)) + cos(Theta(idx))*(YD(idx) - Y(idx)))/Kp;
16
end
17

    
18

    
19
end