Revision 1813
added formations
| branches/16299_s10/matlab/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