Revision 1812
Fixes wheel speed noise, prettier visualization with scaled arrows, and always displays plots
branches/16299_s10/matlab/visualizeRobots.m  

1 
function [mov] = visualizeRobots(f,n,Xs,Ys,Thetas,idx,color,makeMovie,mov,winsize)


1 
function [mov] = visualizeRobots(f,n,Xs,Ys,Thetas,Speeds,idx,color,mov,winsize)


2  2  
3 
% Scales arrows to length 

4 
arrowScale = 1.5; 

5  
3  6 
clf; 
4  7 
ax = [min(min(Xs))1, max(max(Xs))+1, min(min(Ys))1, max(max(Ys))+1]; 
5  8  
...  ...  
23  26 
axis manual 
24  27 
set( gca,'nextplot','add' ); 
25  28 
for j = 1:n 
26 
h = plot_arrow( Xs(j,idx),Ys(j,idx),Xs(j,idx)+cos(Thetas(j,idx)),Ys(j,idx)+sin(Thetas(j,idx)),...


29 
h = plot_arrow( Xs(j,idx),Ys(j,idx),Xs(j,idx) + Speeds(j, idx1) * arrowScale * cos(Thetas(j,idx)),Ys(j,idx) + Speeds(j, idx1) * arrowScale * sin(Thetas(j,idx)),...


27  30 
'color',color(j,:),'facecolor',color(j,:),'edgecolor',color(j,:) ); 
28 
set( h,'linewidth',2 ); 

31 
set( h,'linewidth',3 ); 

32 
d = plot( Xs(j,idx),Ys(j,idx), '.k'); 

33 
set( d,'MarkerSize',5 ); 

29  34 
end 
30  35 
hold off; 
31  36  
32 
if makeMovie == true 

33 
if idx < 3 %so we don't see giant arrows 

34 
clf 

35 
end 

36 
mov(:,idx) = getframe(f,winsize); 

37 
else 

38 
mov = []; 

37 
if idx < 3 %s o we don't see giant arrows 

38 
clf 

39  39 
end 
40 
mov(:,idx) = getframe(f,winsize); 

40  41  
41  42 
end 
branches/16299_s10/matlab/motionModel.m  

2  2  
3  3 
[numRobots,z] = size(v); 
4  4  
5 
wheelMax = [100; 100];


6 
wheelMin = [100; 100];


5 
wheelMax = [4 * pi;4 * pi];


6 
wheelMin = [4 * pi;4 * pi];


7  7  
8 
R = 2.5;


9 
L = 12; 

10 
noise = .5; 

8 
R = 3.5;


9 
L = 12.75;


10 
noise = 0.5;


11  11  
12  12 
transform = [ R/2 R/2; R/L R/L]; 
13  13 
inverseTransform = inv(transform); 
14  14  
15 
% Transforms from v, omega into wheel speed, limits wheel speed and adds noise, then translates back.


16 
for i=1:numRobots,


15 
%Transforms from v, omega into wheel speed, limits wheel speed and adds noise, then translates back. 

16 
for i=2:numRobots,


17  17 

18  18 
wheels = inverseTransform * [v(i); omega(i)]; 
19 
wheels = min(wheels, wheelMax) + noise * (rand  .5);


20 
wheels = max(wheels, wheelMin) + noise * (rand  .5);


19 
wheels = min(wheels, wheelMax) + noise * randn;


20 
wheels = max(wheels, wheelMin) + noise * randn;


21  21 
q = transform * wheels; 
22  22 
v(i) = q(1); 
23  23 
omega(i) = q(2); 
branches/16299_s10/matlab/evolveRobots.m  

1  1 
% n robots 
2  2 
% Robot #1 is the queen 
3 
n = 10; 

4 
dt = 0.05; 

3 
n = 12; 

4 
%for movie output use: 

5 
dt = 0.041709; 

6 
%else: 

7 
%dt = .01; 

5  8 
tf = 40; 
6  9  
7  10 
numsteps = ceil(tf / dt) + 1; 
8  11  
9  12 
%%% output options 
10 
makeMovie = true;


13 
makeMovie = false;


11  14 
showPlots = false; 
12  15 
doFeedback = true; 
13  16  
...  ...  
34  37 
% if we aren't doing feedback just pick random speeds for everyone so 
35  38 
% we can see some open loop motion 
36  39 
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;


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;


41  44 
end 
42  45 
else 
43 
desiredR(:) = 5;


46 
desiredR(:) = 20;


44  47 
desiredPhi = (0:(n1))*(2*pi/(n1)); 
45  48  
46 
V(1,1:end/2) = 0.4;


49 
%V(1,1:end/2) = 1;


47  50 
% 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


51 
for i=2:n 

52 
X(i,1) = rand*15  7.5;


53 
Y(i,1) = rand*15  7.5;


54 
end 

52  55  
53  56  
54  57 
end 
...  ...  
86  89 
winsize = get(f,'Position'); 
87  90 
winsize(1:2) = [0 0]; 
88  91  
89 
if makeMovie == true 

90 
mov = moviein(numsteps+1,f,winsize); 

91 
mov(:,1) = getframe(f,winsize); 

92 
else 

93 
mov = []; 

94 
end 

92 
mov = moviein(numsteps+1,f,winsize); 

93 
mov(:,1) = getframe(f,winsize); 

95  94  
96  95 
% Run through each timestep 
97  96 
for t = 0:dt:tf 
...  ...  
107  106 
%Phis 
108  107  
109  108 
% visualize real position 
110 
mov = visualizeRobots(f,n,X,Y,Theta,idx,color,makeMovie,mov,winsize);


109 
mov = visualizeRobots(f,n,X,Y,Theta,V,idx,color,mov,winsize);


111  110 

112  111 

113  112 
if doFeedback == true 
branches/16299_s10/matlab/sensorModel.m  

1  1 
%TODO: pass in dt for lag model 
2  2 
function [xSensor, ySensor, thetaSensor, phiSensor, state] = sensorModel(xTrue,yTrue,thetaTrue,phiTrue,state) 
3  3  
4 
noiseVar = 0.5;


4 
noiseVar = 0.1;


5  5 
noiseMean = 0; 
6  6  
7  7 
%TODO: value in time instead of number of calls 
branches/16299_s10/matlab/computeTrajectories.m  

2  2 
% This function expects to NOT have the quees be at index 1 
3  3 
function [V,W] = computeTrajectories(X,Y,Theta,XD,YD,ThetaD) 
4  4  
5 
Kp = 5;


5 
Kp = 7;


6  6  
7  7 
% how far away are we 
8  8 
dists = sqrt((XXD).^2 + (YYD) .^ 2); 
branches/16299_s10/code/behaviors/formation_control/hive/hive.c  

197  197 
/* Set the XBee channel to your assigned channel */ /* Set the XBee channel to your assigned channel */ 
198  198 
wl_set_channel(24); 
199  199  
200 
orb_set_color(BLUE); 

201 


202 
encoder_rst_dx(LEFT); 

203 
encoder_rst_dx(RIGHT); 

200  204  
205 
forward(200); 

206 
delay_ms(3000); 

207  
208  
201  209 
orb_set_color(RED); 
202  210 
while(1); /* END HERE */ 
203  211 
Also available in: Unified diff