Revision 1797
fixed vis, quieted motion model, made moving the queen work
branches/16299_s10/matlab/visualizeRobots.m | ||
---|---|---|
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 | ||
---|---|---|
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 | ||
---|---|---|
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 | ||
---|---|---|
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 | ||
---|---|---|
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