root / branches / 16299_s10 / matlab / evolveRobots.m @ 1797
History | View | Annotate | Download (2.98 KB)
1 |
% n robots |
---|---|
2 |
% Robot #1 is the queen |
3 |
n = 10; |
4 |
dt = 0.05; |
5 |
tf = 40; |
6 |
|
7 |
numsteps = ceil(tf / dt) + 1; |
8 |
|
9 |
%%% output options |
10 |
makeMovie = true; |
11 |
showPlots = false; |
12 |
doFeedback = true; |
13 |
|
14 |
|
15 |
X = zeros(n,numsteps); %cm |
16 |
Y = zeros(n,numsteps); %cm |
17 |
R = zeros(n,numsteps); |
18 |
Theta = zeros(n,numsteps); %rads |
19 |
Phi = zeros(n,numsteps); %rads |
20 |
V = zeros(n,numsteps); %0-255 |
21 |
W = zeros(n,numsteps); %0-255 |
22 |
|
23 |
idx = 2; |
24 |
|
25 |
%W(1,1) = pi; |
26 |
%X(2,1) = 2.3; |
27 |
%Y(2,1) = 5.7; |
28 |
|
29 |
|
30 |
desiredR = zeros(n,1); |
31 |
desiredPhi = zeros(n,1); |
32 |
|
33 |
if doFeedback == false |
34 |
% if we aren't doing feedback just pick random speeds for everyone so |
35 |
% we can see some open loop motion |
36 |
for i=1:n |
37 |
X(i,1) = rand(1,1)*10; |
38 |
Y(i,1) = rand(1,1)*10; |
39 |
%V(i,:) = rand(1,1)*10; |
40 |
%W(i,:) = rand(1,1)*2*pi; |
41 |
end |
42 |
else |
43 |
desiredR(:) = 5; |
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 |
|
54 |
end |
55 |
|
56 |
% V(2,:) = 5; |
57 |
% V(3,:) = -10; |
58 |
% V(4,:) = 5; |
59 |
% |
60 |
% W(2,:) = pi; |
61 |
% W(3,:) = 0; |
62 |
% W(4,:) = -pi; |
63 |
|
64 |
|
65 |
color = jet(n); |
66 |
|
67 |
|
68 |
|
69 |
% The things that end in s are the sensor values |
70 |
Xs = zeros(n,numsteps); |
71 |
Ys = zeros(n,numsteps); |
72 |
Thetas = zeros(n,numsteps); |
73 |
Phis = zeros(n,numsteps); |
74 |
|
75 |
% These are the desired values |
76 |
XD = zeros(n,numsteps); |
77 |
YD = zeros(n,numsteps); |
78 |
ThetaD = zeros(n,numsteps); |
79 |
|
80 |
% These state variables allow the models to maintain some state across calls |
81 |
motorState = []; |
82 |
sensorState = []; |
83 |
|
84 |
f = figure; |
85 |
set(f,'NextPlot','replacechildren'); |
86 |
winsize = get(f,'Position'); |
87 |
winsize(1:2) = [0 0]; |
88 |
|
89 |
if makeMovie == true |
90 |
mov = moviein(numsteps+1,f,winsize); |
91 |
mov(:,1) = getframe(f,winsize); |
92 |
else |
93 |
mov = []; |
94 |
end |
95 |
|
96 |
% Run through each timestep |
97 |
for t = 0:dt:tf |
98 |
|
99 |
% update the true positions using the motor model |
100 |
[X(:,idx), Y(:,idx), Theta(:,idx), R(:,idx), Phi(:,idx), motorState] = ... |
101 |
motionModel(V(:,idx-1), W(:,idx-1), X(:,idx-1), Y(:,idx-1), Theta(:,idx-1), dt, motorState); |
102 |
%Phi |
103 |
|
104 |
% Update the sensor values using the sensor model |
105 |
[Xs(:,idx), Ys(:,idx), Thetas(:,idx), Phis(:,idx), sensorState] = ... |
106 |
sensorModel(X(:,idx), Y(:,idx), Theta(:,idx), Phi(:,idx), sensorState); |
107 |
%Phis |
108 |
|
109 |
% visualize real position |
110 |
mov = visualizeRobots(f,n,X,Y,Theta,idx,color,makeMovie,mov,winsize); |
111 |
|
112 |
|
113 |
if doFeedback == true |
114 |
[XD(:,idx), YD(:,idx), ThetaD(:,idx)] = desiredPosition(Xs(:,idx),Ys(:,idx),Phis(:,idx),desiredR,desiredPhi); |
115 |
|
116 |
% ThetaD(:,idx) |
117 |
|
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)); |
120 |
end |
121 |
|
122 |
disp(t); |
123 |
|
124 |
%pause %(dt); |
125 |
|
126 |
idx = idx + 1; |
127 |
end |
128 |
|
129 |
if showPlots |
130 |
figure; |
131 |
hold on; |
132 |
title('true X vs. sensed X'); |
133 |
plot(1:idx-1, X(1,:),1:idx-1, Xs(1,:)) |
134 |
plot(1:idx-1, X(2,:),1:idx-1, Xs(2,:)) |
135 |
end |
136 |
|
137 |
if makeMovie |
138 |
disp('making movie...'); |
139 |
movie2avi(mov,'movie.avi'); |
140 |
disp('movie.avi created!'); |
141 |
end |