root / branches / 16299_s10 / matlab / computeTrajectories.m @ 1821
History | View | Annotate | Download (873 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 | 1816 | bbland | function [V,W] = computeTrajectories(X,Y,Theta,XD,YD,ThetaD,XYError,ThetaError,LastV,LastW) |
4 | 1746 | bneuman | |
5 | 1816 | bbland | Kp = 1/7; |
6 | Ki = 1/100; |
||
7 | Kd = 1/30; |
||
8 | 1746 | bneuman | |
9 | % how far away are we |
||
10 | dists = sqrt((X-XD).^2 + (Y-YD) .^ 2); |
||
11 | |||
12 | 1797 | bneuman | |
13 | 1746 | bneuman | for idx=1:size(X,1) |
14 | %V(idx) = dists(idx)/Kp; % linear velocity is proportional to distance |
||
15 | %W(idx) = (ThetaD(idx) - Theta(idx))/Kp; |
||
16 | 1816 | bbland | curXYError = cos(Theta(idx))*(XD(idx) - X(idx)) + sin(Theta(idx))*(YD(idx) - Y(idx)); |
17 | curThetaError = (-sin(Theta(idx))*(XD(idx) - X(idx)) + cos(Theta(idx))*(YD(idx) - Y(idx))); |
||
18 | XYError(idx) = XYError(idx) + curXYError; |
||
19 | ThetaError(idx) = ThetaError(idx) + curThetaError; |
||
20 | V(idx) = curXYError*Kp + XYError(idx)*Ki + LastV(idx)*Kd; |
||
21 | W(idx) = curThetaError*Kp + ThetaError(idx)*Ki + LastW(idx)*Kd; |
||
22 | 1746 | bneuman | end |
23 | |||
24 | |||
25 | end |