root / branches / 16299_s10 / matlab / evolveRobots.m @ 1812
History  View  Annotate  Download (2.95 KB)
1 
% n robots 

2 
% Robot #1 is the queen 
3 
n = 12; 
4 
%for movie output use: 
5 
dt = 0.041709; 
6 
%else: 
7 
%dt = .01; 
8 
tf = 40; 
9  
10 
numsteps = ceil(tf / dt) + 1; 
11  
12 
%%% output options 
13 
makeMovie = false; 
14 
showPlots = false; 
15 
doFeedback = true; 
16  
17  
18 
X = zeros(n,numsteps); %cm 
19 
Y = zeros(n,numsteps); %cm 
20 
R = zeros(n,numsteps); 
21 
Theta = zeros(n,numsteps); %rads 
22 
Phi = zeros(n,numsteps); %rads 
23 
V = zeros(n,numsteps); %0255 
24 
W = zeros(n,numsteps); %0255 
25 

26 
idx = 2; 
27  
28 
%W(1,1) = pi; 
29 
%X(2,1) = 2.3; 
30 
%Y(2,1) = 5.7; 
31  
32  
33 
desiredR = zeros(n,1); 
34 
desiredPhi = zeros(n,1); 
35  
36 
if doFeedback == false 
37 
% if we aren't doing feedback just pick random speeds for everyone so 
38 
% we can see some open loop motion 
39 
for i=1:n 
40 
X(i,1) = rand*10  5; 
41 
Y(i,1) = rand*10  5; 
42 
V(i,:) = rand*10; 
43 
W(i,:) = rand*2*pi; 
44 
end 
45 
else 
46 
desiredR(:) = 20; 
47 
desiredPhi = (0:(n1))*(2*pi/(n1)); 
48  
49 
%V(1,1:end/2) = 1; 
50 
% start robots at random spots 
51 
for i=2:n 
52 
X(i,1) = rand*15  7.5; 
53 
Y(i,1) = rand*15  7.5; 
54 
end 
55  
56  
57 
end 
58  
59 
% V(2,:) = 5; 
60 
% V(3,:) = 10; 
61 
% V(4,:) = 5; 
62 
% 
63 
% W(2,:) = pi; 
64 
% W(3,:) = 0; 
65 
% W(4,:) = pi; 
66  
67  
68 
color = jet(n); 
69  
70  
71  
72 
% The things that end in s are the sensor values 
73 
Xs = zeros(n,numsteps); 
74 
Ys = zeros(n,numsteps); 
75 
Thetas = zeros(n,numsteps); 
76 
Phis = zeros(n,numsteps); 
77  
78 
% These are the desired values 
79 
XD = zeros(n,numsteps); 
80 
YD = zeros(n,numsteps); 
81 
ThetaD = zeros(n,numsteps); 
82  
83 
% These state variables allow the models to maintain some state across calls 
84 
motorState = []; 
85 
sensorState = []; 
86  
87 
f = figure; 
88 
set(f,'NextPlot','replacechildren'); 
89 
winsize = get(f,'Position'); 
90 
winsize(1:2) = [0 0]; 
91  
92 
mov = moviein(numsteps+1,f,winsize); 
93 
mov(:,1) = getframe(f,winsize); 
94  
95 
% Run through each timestep 
96 
for t = 0:dt:tf 
97 

98 
% update the true positions using the motor model 
99 
[X(:,idx), Y(:,idx), Theta(:,idx), R(:,idx), Phi(:,idx), motorState] = ... 
100 
motionModel(V(:,idx1), W(:,idx1), X(:,idx1), Y(:,idx1), Theta(:,idx1), dt, motorState); 
101 
%Phi 
102 

103 
% Update the sensor values using the sensor model 
104 
[Xs(:,idx), Ys(:,idx), Thetas(:,idx), Phis(:,idx), sensorState] = ... 
105 
sensorModel(X(:,idx), Y(:,idx), Theta(:,idx), Phi(:,idx), sensorState); 
106 
%Phis 
107  
108 
% visualize real position 
109 
mov = visualizeRobots(f,n,X,Y,Theta,V,idx,color,mov,winsize); 
110 

111 

112 
if doFeedback == true 
113 
[XD(:,idx), YD(:,idx), ThetaD(:,idx)] = desiredPosition(Xs(:,idx),Ys(:,idx),Phis(:,idx),desiredR,desiredPhi); 
114 

115 
% ThetaD(:,idx) 
116 

117 
% don't compute for the queen 
118 
[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)); 
119 
end 
120 

121 
disp(t); 
122 

123 
%pause %(dt); 
124 

125 
idx = idx + 1; 
126 
end 
127  
128 
if showPlots 
129 
figure; 
130 
hold on; 
131 
title('true X vs. sensed X'); 
132 
plot(1:idx1, X(1,:),1:idx1, Xs(1,:)) 
133 
plot(1:idx1, X(2,:),1:idx1, Xs(2,:)) 
134 
end 
135  
136 
if makeMovie 
137 
disp('making movie...'); 
138 
movie2avi(mov,'movie.avi'); 
139 
disp('movie.avi created!'); 
140 
end 