Revision 1797 branches/16299_s10/matlab/evolveRobots.m
| evolveRobots.m (revision 1797) | ||
|---|---|---|
| 1 |
% 4 robots |
|
| 1 |
% n robots |
|
| 2 | 2 |
% Robot #1 is the queen |
| 3 | 3 |
n = 10; |
| 4 | 4 |
dt = 0.05; |
| 5 |
tf = 10; |
|
| 5 |
tf = 40; |
|
| 6 | 6 |
|
| 7 | 7 |
numsteps = ceil(tf / dt) + 1; |
| 8 | 8 |
|
| ... | ... | |
| 36 | 36 |
for i=1:n |
| 37 | 37 |
X(i,1) = rand(1,1)*10; |
| 38 | 38 |
Y(i,1) = rand(1,1)*10; |
| 39 |
V(i,:) = rand(1,1)*10; |
|
| 40 |
W(i,:) = rand(1,1)*2*pi; |
|
| 39 |
%V(i,:) = rand(1,1)*10; |
|
| 40 |
%W(i,:) = rand(1,1)*2*pi; |
|
| 41 | 41 |
end |
| 42 | 42 |
else |
| 43 | 43 |
desiredR(:) = 5; |
| 44 | 44 |
desiredPhi = (0:(n-1))*(2*pi/(n-1)); |
| 45 |
|
|
| 46 |
V(1,1:end/2) = 0.4; |
|
| 47 |
% start robots at random spots |
|
| 48 |
% for i=2:n |
|
| 49 |
% X(i,1) = rand(1,1)*10; |
|
| 50 |
% Y(i,1) = rand(1,1)*10; |
|
| 51 |
% end |
|
| 52 |
|
|
| 53 |
|
|
| 45 | 54 |
end |
| 46 | 55 |
|
| 47 | 56 |
% V(2,:) = 5; |
| ... | ... | |
| 97 | 106 |
sensorModel(X(:,idx), Y(:,idx), Theta(:,idx), Phi(:,idx), sensorState); |
| 98 | 107 |
%Phis |
| 99 | 108 |
|
| 100 |
mov = visualizeRobots(f,n,Xs,Ys,Thetas,idx,color,makeMovie,mov,winsize); |
|
| 109 |
% visualize real position |
|
| 110 |
mov = visualizeRobots(f,n,X,Y,Theta,idx,color,makeMovie,mov,winsize); |
|
| 101 | 111 |
|
| 102 | 112 |
|
| 103 | 113 |
if doFeedback == true |
| ... | ... | |
| 105 | 115 |
|
| 106 | 116 |
% ThetaD(:,idx) |
| 107 | 117 |
|
| 108 |
[V(:,idx),W(:,idx)] = computeTrajectories(Xs(:,idx),Ys(:,idx),Thetas(:,idx),XD(:,idx),YD(:,idx),ThetaD(:,idx)); |
|
| 118 |
% don't compute for the queen |
|
| 119 |
[V(2:end,idx),W(2:end,idx)] = computeTrajectories(Xs(2:end,idx),Ys(2:end,idx),Thetas(2:end,idx),XD(2:end,idx),YD(2:end,idx),ThetaD(2:end,idx)); |
|
| 109 | 120 |
end |
| 110 | 121 |
|
| 111 | 122 |
disp(t); |
Also available in: Unified diff