Revision 1797
| branches/16299_s10/matlab/visualizeRobots.m (revision 1797) | ||
|---|---|---|
| 1 | 1 |
function [mov] = visualizeRobots(f,n,Xs,Ys,Thetas,idx,color,makeMovie,mov,winsize) |
| 2 | 2 |
|
| 3 | 3 |
clf; |
| 4 |
axis([-10 10 -10 10]) |
|
| 4 |
ax = [min(min(Xs))-1, max(max(Xs))+1, min(min(Ys))-1, max(max(Ys))+1]; |
|
| 5 |
|
|
| 6 |
if ax(1) > -10 |
|
| 7 |
ax(1) = -10; |
|
| 8 |
end |
|
| 9 |
if ax(2) < 10 |
|
| 10 |
ax(2) = 10; |
|
| 11 |
end |
|
| 12 |
if ax(3) > -10 |
|
| 13 |
ax(3) = -10; |
|
| 14 |
end |
|
| 15 |
if ax(4) < 10 |
|
| 16 |
ax(4) = 10; |
|
| 17 |
end |
|
| 18 |
|
|
| 19 |
axis(ax); |
|
| 20 |
|
|
| 21 |
%axis([-10 10 -10 10]) |
|
| 22 |
|
|
| 5 | 23 |
axis manual |
| 6 | 24 |
set( gca,'nextplot','add' ); |
| 7 | 25 |
for j = 1:n |
| branches/16299_s10/matlab/motionModel.m (revision 1797) | ||
|---|---|---|
| 15 | 15 |
|
| 16 | 16 |
wheels = inverseTransform * [v(i); omega(i)]; |
| 17 | 17 |
wheels = min(wheels, wheelMax); |
| 18 |
wheels = max(wheels, wheelMin) |
|
| 19 |
q = transform * wheels |
|
| 18 |
wheels = max(wheels, wheelMin); |
|
| 19 |
q = transform * wheels; |
|
| 20 | 20 |
v(i) = q(1); |
| 21 | 21 |
omega(i) = q(2); |
| 22 | 22 |
|
| branches/16299_s10/matlab/evolveRobots.m (revision 1797) | ||
|---|---|---|
| 1 |
% 4 robots |
|
| 1 |
% n robots |
|
| 2 | 2 |
% Robot #1 is the queen |
| 3 | 3 |
n = 10; |
| 4 | 4 |
dt = 0.05; |
| 5 |
tf = 10; |
|
| 5 |
tf = 40; |
|
| 6 | 6 |
|
| 7 | 7 |
numsteps = ceil(tf / dt) + 1; |
| 8 | 8 |
|
| ... | ... | |
| 36 | 36 |
for i=1:n |
| 37 | 37 |
X(i,1) = rand(1,1)*10; |
| 38 | 38 |
Y(i,1) = rand(1,1)*10; |
| 39 |
V(i,:) = rand(1,1)*10; |
|
| 40 |
W(i,:) = rand(1,1)*2*pi; |
|
| 39 |
%V(i,:) = rand(1,1)*10; |
|
| 40 |
%W(i,:) = rand(1,1)*2*pi; |
|
| 41 | 41 |
end |
| 42 | 42 |
else |
| 43 | 43 |
desiredR(:) = 5; |
| 44 | 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 |
|
|
| 45 | 54 |
end |
| 46 | 55 |
|
| 47 | 56 |
% V(2,:) = 5; |
| ... | ... | |
| 97 | 106 |
sensorModel(X(:,idx), Y(:,idx), Theta(:,idx), Phi(:,idx), sensorState); |
| 98 | 107 |
%Phis |
| 99 | 108 |
|
| 100 |
mov = visualizeRobots(f,n,Xs,Ys,Thetas,idx,color,makeMovie,mov,winsize); |
|
| 109 |
% visualize real position |
|
| 110 |
mov = visualizeRobots(f,n,X,Y,Theta,idx,color,makeMovie,mov,winsize); |
|
| 101 | 111 |
|
| 102 | 112 |
|
| 103 | 113 |
if doFeedback == true |
| ... | ... | |
| 105 | 115 |
|
| 106 | 116 |
% ThetaD(:,idx) |
| 107 | 117 |
|
| 108 |
[V(:,idx),W(:,idx)] = computeTrajectories(Xs(:,idx),Ys(:,idx),Thetas(:,idx),XD(:,idx),YD(:,idx),ThetaD(:,idx)); |
|
| 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)); |
|
| 109 | 120 |
end |
| 110 | 121 |
|
| 111 | 122 |
disp(t); |
| branches/16299_s10/matlab/sensorModel.m (revision 1797) | ||
|---|---|---|
| 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; |
|
| 5 |
noiseMean = 0; |
|
| 6 |
|
|
| 4 | 7 |
%TODO: value in time instead of number of calls |
| 5 | 8 |
sensorLag = 10; |
| 6 | 9 |
|
| ... | ... | |
| 21 | 24 |
|
| 22 | 25 |
% phiSensor is the value from the BOM sensor |
| 23 | 26 |
% round past phi to the nearest pi/8 |
| 24 |
noisePhi = state.phi(1,:)' + randn() - 0.5; |
|
| 27 |
noisePhi = state.phi(1,:)' + randn()*noiseVar + noiseMean; |
|
| 25 | 28 |
phiSensor = round(noisePhi*8/pi)*pi/8; |
| 26 | 29 |
%phiSensor = round(state.phi(1,:)'*8/pi)*pi/8; |
| 27 | 30 |
|
| branches/16299_s10/matlab/computeTrajectories.m (revision 1797) | ||
|---|---|---|
| 1 | 1 |
% For now I am ignoring ThetaD and just computing an arc |
| 2 |
% This function expects to NOT have the quees be at index 1 |
|
| 2 | 3 |
function [V,W] = computeTrajectories(X,Y,Theta,XD,YD,ThetaD) |
| 3 | 4 |
|
| 4 | 5 |
Kp = 5; |
| ... | ... | |
| 6 | 7 |
% how far away are we |
| 7 | 8 |
dists = sqrt((X-XD).^2 + (Y-YD) .^ 2); |
| 8 | 9 |
|
| 10 |
|
|
| 9 | 11 |
for idx=1:size(X,1) |
| 10 | 12 |
%V(idx) = dists(idx)/Kp; % linear velocity is proportional to distance |
| 11 | 13 |
%W(idx) = (ThetaD(idx) - Theta(idx))/Kp; |
Also available in: Unified diff