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, idx-1) * arrowScale * cos(Thetas(j,idx)),Ys(j,idx) + Speeds(j, idx-1) * 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:(n-1))*(2*pi/(n-1)); |
| 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((X-XD).^2 + (Y-YD) .^ 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