Project

General

Profile

Revision 1812

Added by Chase Brownell over 7 years ago

Fixes wheel speed noise, prettier visualization with scaled arrows, and always displays plots

View differences:

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