Revision 1797
fixed vis, quieted motion model, made moving the queen work
evolveRobots.m | ||
---|---|---|
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