Project

General

Profile

Revision 1797

fixed vis, quieted motion model, made moving the queen work

View differences:

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