root / branches / 16299_s10 / matlab / evolveRobots.m @ 1822
History | View | Annotate | Download (5.88 KB)
1 |
% n robots |
---|---|
2 |
% Robot #1 is the queen |
3 |
n = 15; |
4 |
%for movie output use: |
5 |
dt = 0.041709; |
6 |
%else: |
7 |
%dt = .1; |
8 |
tf = 20 %40; |
9 |
|
10 |
|
11 |
%shape numbers: |
12 |
%0 = nothing |
13 |
%1 = circle |
14 |
%2 = square |
15 |
%3 = diamond |
16 |
%4 = arrow |
17 |
%5 = semicircles |
18 |
|
19 |
numsteps = ceil(tf / dt) + 1; |
20 |
|
21 |
%%% output options |
22 |
makeMovie = true; |
23 |
showPlots = false; |
24 |
doFeedback = true; |
25 |
shape = 1; |
26 |
|
27 |
if shape == 1 |
28 |
n = 65; |
29 |
elseif shape == 2 |
30 |
n = 65; |
31 |
elseif shape == 3 |
32 |
n = 65; |
33 |
elseif shape == 4 |
34 |
n = 65; |
35 |
elseif shape == 5 |
36 |
n = 20; |
37 |
end |
38 |
|
39 |
X = zeros(n,numsteps); %cm |
40 |
Y = zeros(n,numsteps); %cm |
41 |
R = zeros(n,numsteps); |
42 |
Theta = zeros(n,numsteps); %rads |
43 |
Phi = zeros(n,numsteps); %rads |
44 |
V = zeros(n,numsteps); %0-255 |
45 |
W = zeros(n,numsteps); %0-255 |
46 |
|
47 |
idx = 2; |
48 |
|
49 |
%W(1,1) = pi; |
50 |
%X(2,1) = 2.3; |
51 |
%Y(2,1) = 5.7; |
52 |
|
53 |
|
54 |
desiredR = zeros(n,1); |
55 |
desiredPhi = zeros(n,1); |
56 |
|
57 |
if doFeedback == false |
58 |
% if we aren't doing feedback just pick random speeds for everyone so |
59 |
% we can see some open loop motion |
60 |
%HACK |
61 |
%V(2,:) = 2; |
62 |
for i=2:n |
63 |
%X(i,1) = rand*10 - 5; |
64 |
Y(i,1) = -2.5; |
65 |
V(i,:) = 6; |
66 |
W(i,:) = 2.5; |
67 |
end |
68 |
else |
69 |
|
70 |
if shape == 1 |
71 |
%circle |
72 |
desiredR(:) = 20; |
73 |
desiredPhi = (0:(n-1))*(2*pi/(n-1)); |
74 |
|
75 |
elseif shape == 2 |
76 |
%square |
77 |
desiredPhi = (0:(n-1))*(2*pi/(n-1))+pi/2; |
78 |
for index=2:(n-1)/4 |
79 |
desiredR(index) = 20/cos(abs(desiredPhi(index)-3*pi/4)); |
80 |
end |
81 |
for index=1+(n-1)/4:(n-1)/2 |
82 |
desiredR(index) = 20/sin(abs(desiredPhi(index)-3*pi/4)); |
83 |
end |
84 |
for index=1+(n-1)/2:3*(n-1)/4 |
85 |
desiredR(index) = 20/sin(abs(desiredPhi(index)+3*pi/4)); |
86 |
end |
87 |
for index=1+3*(n-1)/4:n-1 |
88 |
desiredR(index) = abs(20/sin(abs(desiredPhi(index)-3*pi/4))); |
89 |
end |
90 |
desiredR(n) = 20*sqrt(2); |
91 |
desiredPhi = desiredPhi-pi/4; |
92 |
|
93 |
elseif shape == 3 |
94 |
%diamond |
95 |
desiredPhi = (0:(n-1))*(2*pi/(n-1))+pi/2; |
96 |
for index=2:(n-1)/4 |
97 |
desiredR(index) = 20/cos(abs(desiredPhi(index)-3*pi/4)); |
98 |
end |
99 |
for index=1+(n-1)/4:(n-1)/2 |
100 |
desiredR(index) = 20/sin(abs(desiredPhi(index)-3*pi/4)); |
101 |
end |
102 |
for index=1+(n-1)/2:3*(n-1)/4 |
103 |
desiredR(index) = 20/sin(abs(desiredPhi(index)+3*pi/4)); |
104 |
end |
105 |
for index=1+3*(n-1)/4:n-1 |
106 |
desiredR(index) = abs(20/sin(abs(desiredPhi(index)-3*pi/4))); |
107 |
end |
108 |
desiredR(n) = 20*sqrt(2); |
109 |
|
110 |
elseif shape == 4 |
111 |
%arrow |
112 |
desiredPhi(2:(n-1)/4) = pi; |
113 |
desiredPhi(1+(n-1)/4:(n-1)/2) = 0; |
114 |
|
115 |
for index=1+(n-1)/2:n |
116 |
desiredPhi(index) = index*(2*pi/(n-1))+pi/2; |
117 |
end |
118 |
for index=2:(n-1)/4 |
119 |
desiredR(index) = index*80*sqrt(2)/(n-1); |
120 |
desiredR(index+(n-1)/4) = desiredR(index); |
121 |
end |
122 |
for index=1+(n-1)/2:3*(n-1)/4 |
123 |
desiredR(index) = 20/sin(abs(desiredPhi(index)+3*pi/4)); |
124 |
end |
125 |
for index=1+3*(n-1)/4:n-1 |
126 |
desiredR(index) = abs(20/sin(abs(desiredPhi(index)-3*pi/4))); |
127 |
end |
128 |
|
129 |
elseif shape == 5 |
130 |
%2 circles |
131 |
desiredR(1:(n/2)) = 20; |
132 |
desiredR((n/2)+1:n) = 40; |
133 |
desiredPhi(1:(n/2)) = (1:(n/2))*(2*pi/(n-1)); |
134 |
desiredPhi((n/2)+1:n) =(1:(n/2))*(2*pi/(n-1))+pi/3; |
135 |
|
136 |
end |
137 |
|
138 |
|
139 |
V(1,end/2:3*end/4) = 1; |
140 |
%changing orientation of the queen |
141 |
%W(1,floor(end/6):floor(end/6)+20) = pi/2; |
142 |
%W(1,floor(end/3):floor(end/3)+20) = -1*pi/2; |
143 |
% start robots at random spots |
144 |
%for i=2:n |
145 |
% X(i,1) = rand*15 - 7.5; |
146 |
% Y(i,1) = rand*15 - 7.5; |
147 |
%end |
148 |
|
149 |
|
150 |
end |
151 |
|
152 |
% V(2,:) = 5; |
153 |
% V(3,:) = -10; |
154 |
% V(4,:) = 5; |
155 |
% |
156 |
% W(2,:) = pi; |
157 |
% W(3,:) = 0; |
158 |
% W(4,:) = -pi; |
159 |
|
160 |
|
161 |
color = hsv(n); |
162 |
|
163 |
encoderNoise = .01 * randn(2, n); |
164 |
|
165 |
|
166 |
% The things that end in s are the sensor values |
167 |
Xs = zeros(n,numsteps); |
168 |
Ys = zeros(n,numsteps); |
169 |
Thetas = zeros(n,numsteps); |
170 |
Phis = zeros(n,numsteps); |
171 |
|
172 |
% These are the desired values |
173 |
XD = zeros(n,numsteps); |
174 |
YD = zeros(n,numsteps); |
175 |
ThetaD = zeros(n,numsteps); |
176 |
|
177 |
% These state variables allow the models to maintain some state across calls |
178 |
motorState = []; |
179 |
sensorState = []; |
180 |
|
181 |
%Running error count for integral control |
182 |
XYError = zeros(n,numsteps); |
183 |
ThetaError = zeros(n,numsteps); |
184 |
|
185 |
f = figure; |
186 |
set(f,'NextPlot','replacechildren'); |
187 |
winsize = get(f,'Position'); |
188 |
winsize(1:2) = [0 0]; |
189 |
|
190 |
mov = moviein(numsteps+1,f,winsize); |
191 |
mov(:,1) = getframe(f,winsize); |
192 |
|
193 |
% init |
194 |
xoldSensor = 0; |
195 |
yoldSensor = 0; |
196 |
thetaoldSensor = 0; |
197 |
noise = 0; |
198 |
|
199 |
% Run through each timestep |
200 |
for t = 0:dt:tf |
201 |
|
202 |
|
203 |
|
204 |
% update the true positions using the motor model |
205 |
[X(:,idx), Y(:,idx), Theta(:,idx), Phi(:,idx), motorState, wheels] = ... |
206 |
motionModel(V(:,idx-1), W(:,idx-1), X(:,idx-1), Y(:,idx-1), Theta(:,idx-1), dt, motorState, n); |
207 |
%Phi |
208 |
|
209 |
% Update the sensor values using the sensor model |
210 |
[Xs(:,idx), Ys(:,idx), Thetas(:,idx), Phis(:,idx), sensorState, encoderNoise, xoldSensor, yoldSensor, thetaoldSensor, noise] = ... |
211 |
sensorModel(X(:,idx), Y(:,idx), Theta(:,idx), Phi(:,idx), sensorState, n, encoderNoise, wheels, xoldSensor, yoldSensor, thetaoldSensor, dt, noise); |
212 |
%Phis |
213 |
|
214 |
% visualize real position |
215 |
mov = visualizeRobots(f,n,X,Y,Theta,V,idx,color,mov,winsize); |
216 |
|
217 |
|
218 |
if doFeedback == true |
219 |
[XD(:,idx), YD(:,idx), ThetaD(:,idx)] = desiredPosition(Xs(:,idx),Ys(:,idx),Phis(:,idx),desiredR,desiredPhi); |
220 |
|
221 |
% ThetaD(:,idx) |
222 |
|
223 |
% don't compute for the queen |
224 |
[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),XYError(2:end,idx),ThetaError(2:end,idx),V(2:end,idx-1),W(2:end,idx-1)); |
225 |
end |
226 |
|
227 |
disp(t); |
228 |
|
229 |
%pause %(dt); |
230 |
|
231 |
idx = idx + 1; |
232 |
end |
233 |
|
234 |
if showPlots |
235 |
figure; |
236 |
hold on; |
237 |
%title('true X vs. sensed X'); |
238 |
% plot(1:idx-1, X(1,:),1:idx-1, Xs(1,:)) |
239 |
%plot(1:idx-1, X(2,:) - Xs(2,:),1:idx-1, ) |
240 |
%legend('true x', 'sensed x') |
241 |
|
242 |
title('Error'); |
243 |
% plot(1:idx-1, X(1,:),1:idx-1, Xs(1,:)) |
244 |
for i=2:n |
245 |
plot(0:dt:(tf+dt), abs(X(i,:)) .* abs(Y(i,:)) - abs(Xs(i,:)) .* abs(Ys(i,:))) |
246 |
end |
247 |
%legend('true x', 'sensed x') |
248 |
end |
249 |
|
250 |
if makeMovie |
251 |
disp('making movie...'); |
252 |
movie2avi(mov,'movie.avi', 'fps', 23.976); |
253 |
disp('movie.avi created!'); |
254 |
end |