Project

General

Profile

Revision 1822

Added by Chase Brownell almost 14 years ago

don't even know

View differences:

branches/16299_s10/matlab/visualizeRobots.m
38 38
if idx < 3 %s o we don't see giant arrows
39 39
    clf
40 40
end
41
%mov(:,idx) = getframe(f,winsize);
41
mov(:,idx) = getframe(f,winsize);
42 42
pause(.001);
43 43

  
44 44
end
branches/16299_s10/matlab/motionModel.m
7 7

  
8 8
R = 3.5;
9 9
L = 12.75;
10
noise = 0.5;
10
noise = 0.1;
11 11

  
12 12
transform = [ R/2 R/2; -R/L R/L];
13 13
inverseTransform = inv(transform);
......
36 36
y = yold + sin(thetaold).*v*dt;
37 37
theta = thetaold + omega*dt;
38 38

  
39
disp(x)
40 39
    
41 40
phi = theta - atan2(y-y(1), x-x(1));
42 41

  
branches/16299_s10/matlab/evolveRobots.m
1 1
% n robots
2 2
% Robot #1 is the queen
3
n = 13;
3
n = 15;
4 4
%for movie output use:
5
 dt = 0.041709;
5
dt = 0.041709;
6 6
%else:
7
%dt = .01;
7
%dt = .1;
8 8
tf = 20 %40;
9 9

  
10 10

  
......
19 19
numsteps = ceil(tf / dt) + 1;
20 20

  
21 21
%%% output options
22
makeMovie = false;
23
showPlots = true;
22
makeMovie = true;
23
showPlots = false;
24 24
doFeedback = true;
25 25
shape = 1;
26 26

  
27 27
if shape == 1
28 28
    n = 65;
29
elseif shape < 4
29
elseif shape == 2
30 30
    n = 65;
31
elseif shape == 3
32
    n = 65;
31 33
elseif shape == 4
32 34
    n = 65;
33 35
elseif shape == 5
......
56 58
    % if we aren't doing feedback just pick random speeds for everyone so
57 59
    % we can see some open loop motion
58 60
    %HACK
59
    V(2,:) = 2;
60
    for i=3:n
61
        X(i,1) = rand*10 - 5;
62
        Y(i,1) = rand*10 - 5;
63
        V(i,:) = rand*10;
64
        W(i,:) = rand*2*pi;
61
    %V(2,:) = 2;
62
    for i=2:n
63
        %X(i,1) = rand*10 - 5;
64
        Y(i,1) =  -2.5;
65
        V(i,:) = 6;
66
        W(i,:) = 2.5;
65 67
    end
66 68
else
67 69

  
......
158 160

  
159 161
color = hsv(n);
160 162

  
161
encoderNoise = 3 * randn(2, n);
163
encoderNoise = .01 * randn(2, n);
162 164

  
163 165

  
164 166
% The things that end in s are the sensor values
......
232 234
if showPlots
233 235
    figure;
234 236
    hold on;
235
    title('true X vs. sensed X');
237
    %title('true X vs. sensed X');
236 238
%    plot(1:idx-1, X(1,:),1:idx-1, Xs(1,:)) 
237
    plot(1:idx-1, X(2,:),1:idx-1, Xs(2,:)) 
239
    %plot(1:idx-1, X(2,:) - Xs(2,:),1:idx-1, ) 
240
	%legend('true x', 'sensed x')
241
	
242
	title('Error');
243
%    plot(1:idx-1, X(1,:),1:idx-1, Xs(1,:)) 
244
    for i=2:n
245
		plot(0:dt:(tf+dt), abs(X(i,:)) .* abs(Y(i,:)) - abs(Xs(i,:)) .* abs(Ys(i,:))) 
246
	end
247
	%legend('true x', 'sensed x')
238 248
end
239 249

  
240 250
if makeMovie
241 251
    disp('making movie...');
242
    movie2avi(mov,'movie.avi');
252
    movie2avi(mov,'movie.avi', 'fps', 23.976);
243 253
    disp('movie.avi created!');
244 254
end
branches/16299_s10/matlab/sensorModel.m
2 2
function [xSensor, ySensor, thetaSensor, phiSensor, state, encoderNoise, xoldSensed, yoldSensed, thetaoldSensed, noise] = sensorModel(xTrue,yTrue,thetaTrue,phiTrue,state, n, encoderNoise, wheels, xoldSensed, yoldSensed, thetaoldSensed, dt, noise)
3 3

  
4 4
phiNoiseVar = 0.1;
5
encoderNoiseVar = 0.05;
5
encoderNoiseVar = 0.001;
6 6
noiseMean = 0;
7 7

  
8 8
%TODO: value in time instead of number of calls
......
20 20
% Use the lagged values for position
21 21

  
22 22

  
23
noise = noise + (encoderNoise .* (encoderNoiseVar * abs(randn(2, n))) ./ wheels);
23
noise = noise + ((encoderNoise .* (encoderNoiseVar * abs(randn(2, n)))) .* wheels);
24 24

  
25 25

  
26 26
R = 3.5;
......
41 41
	
42 42
end
43 43

  
44
disp(wheels)
45 44

  
46 45
xSensor = xoldSensed;
47 46
ySensor = yoldSensed;

Also available in: Unified diff