Revision 1813
added formations
evolveRobots.m | ||
---|---|---|
7 | 7 |
%dt = .01; |
8 | 8 |
tf = 40; |
9 | 9 |
|
10 |
|
|
11 |
%shape numbers: |
|
12 |
%0 = nothing |
|
13 |
%1 = circle |
|
14 |
%2 = square |
|
15 |
%3 = diamond |
|
16 |
%4 = arrow |
|
17 |
%5 = semicircles |
|
18 |
|
|
10 | 19 |
numsteps = ceil(tf / dt) + 1; |
11 | 20 |
|
12 | 21 |
%%% output options |
13 | 22 |
makeMovie = false; |
14 | 23 |
showPlots = false; |
15 | 24 |
doFeedback = true; |
25 |
shape = 1; |
|
16 | 26 |
|
27 |
if shape == 1 |
|
28 |
n = 100; |
|
29 |
elseif shape < 4 |
|
30 |
n = 9; |
|
31 |
elseif shape == 4 |
|
32 |
n = 9; |
|
33 |
elseif shape == 5 |
|
34 |
n = 20; |
|
35 |
end |
|
17 | 36 |
|
18 | 37 |
X = zeros(n,numsteps); %cm |
19 | 38 |
Y = zeros(n,numsteps); %cm |
... | ... | |
43 | 62 |
W(i,:) = rand*2*pi; |
44 | 63 |
end |
45 | 64 |
else |
65 |
|
|
66 |
if shape == 1 |
|
67 |
%circle |
|
46 | 68 |
desiredR(:) = 20; |
47 | 69 |
desiredPhi = (0:(n-1))*(2*pi/(n-1)); |
48 |
|
|
70 |
|
|
71 |
elseif shape == 2 |
|
72 |
%square |
|
73 |
desiredR(2*(1:(n-1))+1) = 20; |
|
74 |
desiredR(2*(1:(n-1))) = 20*sqrt(2); |
|
75 |
desiredPhi = (0:(n-1))*pi/4; |
|
76 |
|
|
77 |
elseif shape == 3 |
|
78 |
%diamond |
|
79 |
desiredR(2*(1:(n-1))) = 20; |
|
80 |
desiredR(2*(1:(n-1))+1) = 20*sqrt(2); |
|
81 |
desiredPhi = (0:(n-1))*pi/4; |
|
82 |
|
|
83 |
elseif shape == 4 |
|
84 |
%aarow |
|
85 |
desiredR(2:4) = 20; |
|
86 |
desiredR(5) = 13; |
|
87 |
desiredR(6) = 24; |
|
88 |
desiredR(7) = 36; |
|
89 |
desiredR(8) = 17; |
|
90 |
desiredR(9) = 17; |
|
91 |
desiredPhi(2) = 0; |
|
92 |
desiredPhi(3) = pi/2; |
|
93 |
desiredPhi(4) = 3*pi/2; |
|
94 |
desiredPhi(5:7) = pi; |
|
95 |
desiredPhi(8) = pi/4; |
|
96 |
desiredPhi(9) = 7*pi/4; |
|
97 |
|
|
98 |
elseif shape == 5 |
|
99 |
%2 circles |
|
100 |
desiredR(1:(n/2)) = 20; |
|
101 |
desiredR((n/2)+1:n) = 40; |
|
102 |
desiredPhi(1:(n/2)) = (1:(n/2))*(2*pi/(n-1)); |
|
103 |
desiredPhi((n/2)+1:n) =(1:(n/2))*(2*pi/(n-1))+pi/3; |
|
104 |
|
|
105 |
end |
|
106 |
|
|
107 |
|
|
49 | 108 |
%V(1,1:end/2) = 1; |
109 |
%changing orientation of the queen |
|
110 |
W(1,floor(end/6):floor(end/6)+20) = pi/2; |
|
111 |
W(1,floor(end/3):floor(end/3)+20) = -1*pi/2; |
|
50 | 112 |
% start robots at random spots |
51 | 113 |
for i=2:n |
52 | 114 |
X(i,1) = rand*15 - 7.5; |
... | ... | |
95 | 157 |
% Run through each timestep |
96 | 158 |
for t = 0:dt:tf |
97 | 159 |
|
160 |
|
|
161 |
|
|
98 | 162 |
% update the true positions using the motor model |
99 | 163 |
[X(:,idx), Y(:,idx), Theta(:,idx), R(:,idx), Phi(:,idx), motorState] = ... |
100 | 164 |
motionModel(V(:,idx-1), W(:,idx-1), X(:,idx-1), Y(:,idx-1), Theta(:,idx-1), dt, motorState); |
Also available in: Unified diff