root / branches / 16299_s10 / matlab / computeTrajectories.m @ 1812
History | View | Annotate | Download (583 Bytes)
1 | 1746 | bneuman | % For now I am ignoring ThetaD and just computing an arc |
---|---|---|---|
2 | 1797 | bneuman | % This function expects to NOT have the quees be at index 1 |
3 | 1746 | bneuman | function [V,W] = computeTrajectories(X,Y,Theta,XD,YD,ThetaD) |
4 | |||
5 | 1812 | cbrownel | Kp = 7; |
6 | 1746 | bneuman | |
7 | % how far away are we |
||
8 | dists = sqrt((X-XD).^2 + (Y-YD) .^ 2); |
||
9 | |||
10 | 1797 | bneuman | |
11 | 1746 | bneuman | 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 |