root / branches / 16299_s10 / matlab / desiredPosition.m @ 1812
History | View | Annotate | Download (1.05 KB)
1 |
function [ desX, desY, desPhi ] = desiredPosition( curX, curY, curPhi, desR, desPhi ) |
---|---|
2 |
%function [ desX, desY, desTh ] = desiredPosition( curX, curY, curTh, desR, desPhi ) |
3 |
|
4 |
desX = zeros(size(curX)); |
5 |
desY = zeros(size(curX)); |
6 |
desTh = zeros(size(curX)); |
7 |
|
8 |
for i=2:size(curX,1) |
9 |
|
10 |
desX(i) = desR(i)*cos(desPhi(i) - curPhi(1)) + curX(1); |
11 |
desY(i) = desR(i)*sin(desPhi(i) - curPhi(1)) + curY(1); |
12 |
% desX(i) = desR(i)*cos(desPhi(i) - curTh(1)) + curX(1); |
13 |
% desY(i) = desR(i)*sin(desPhi(i) - curTh(1)) + curY(1); |
14 |
%desTh(i) = atan2(desY(i)-curY(i), desX(i)-curX(i)); |
15 |
|
16 |
|
17 |
% rotate and translate so curr pos is 0,0 and cur theta = 0 |
18 |
% rotate by -curTh(i) |
19 |
% x = (desX(i)-curX(i))*cos(-curTh(i)) - (desY(i)-curY(i))*sin(-curTh(i)); |
20 |
% y = (desX(i)-curX(i))*sin(-curTh(i)) + (desY(i)-curY(i))*cos(-curTh(i)); |
21 |
% |
22 |
% d = sqrt(x^2 + y^2) |
23 |
% p = atan2(y, x) |
24 |
% r = d/(2*sin(0.5*(pi-2*p))) |
25 |
% |
26 |
% (d*sin(p)-r)/r; |
27 |
% desTh(i) = asin((d*sin(p)-r)/r) + curTh(i); |
28 |
|
29 |
desPhi(i) = 0; |
30 |
%desTh(i) = 0; %for now, don't command theta |
31 |
end |
32 |
|
33 |
end |