Statistics
| Revision:

root / branches / 16299_s10 / matlab / evolveRobots.m @ 1812

 1 ```% n robots ``` ```% Robot #1 is the queen ``` ```n = 12; ``` ```%for movie output use: ``` ``` dt = 0.041709; ``` ```%else: ``` ```%dt = .01; ``` ```tf = 40; ``` ```numsteps = ceil(tf / dt) + 1; ``` ```%%% output options ``` ```makeMovie = false; ``` ```showPlots = false; ``` ```doFeedback = true; ``` ```X = zeros(n,numsteps); %cm ``` ```Y = zeros(n,numsteps); %cm ``` ```R = zeros(n,numsteps); ``` ```Theta = zeros(n,numsteps); %rads ``` ```Phi = zeros(n,numsteps); %rads ``` ```V = zeros(n,numsteps); %0-255 ``` ```W = zeros(n,numsteps); %0-255 ``` ``` ``` ```idx = 2; ``` ```%W(1,1) = pi; ``` ```%X(2,1) = 2.3; ``` ```%Y(2,1) = 5.7; ``` ```desiredR = zeros(n,1); ``` ```desiredPhi = zeros(n,1); ``` ```if doFeedback == false ``` ``` % if we aren't doing feedback just pick random speeds for everyone so ``` ``` % we can see some open loop motion ``` ``` for i=1:n ``` ``` X(i,1) = rand*10 - 5; ``` ``` Y(i,1) = rand*10 - 5; ``` ``` V(i,:) = rand*10; ``` ``` W(i,:) = rand*2*pi; ``` ``` end ``` ```else ``` ``` desiredR(:) = 20; ``` ``` desiredPhi = (0:(n-1))*(2*pi/(n-1)); ``` ``` %V(1,1:end/2) = 1; ``` ``` % start robots at random spots ``` ``` for i=2:n ``` ``` X(i,1) = rand*15 - 7.5; ``` ``` Y(i,1) = rand*15 - 7.5; ``` ``` end ``` ```end ``` ```% V(2,:) = 5; ``` ```% V(3,:) = -10; ``` ```% V(4,:) = 5; ``` ```% ``` ```% W(2,:) = pi; ``` ```% W(3,:) = 0; ``` ```% W(4,:) = -pi; ``` ```color = jet(n); ``` ```% The things that end in s are the sensor values ``` ```Xs = zeros(n,numsteps); ``` ```Ys = zeros(n,numsteps); ``` ```Thetas = zeros(n,numsteps); ``` ```Phis = zeros(n,numsteps); ``` ```% These are the desired values ``` ```XD = zeros(n,numsteps); ``` ```YD = zeros(n,numsteps); ``` ```ThetaD = zeros(n,numsteps); ``` ```% These state variables allow the models to maintain some state across calls ``` ```motorState = []; ``` ```sensorState = []; ``` ```f = figure; ``` ```set(f,'NextPlot','replacechildren'); ``` ```winsize = get(f,'Position'); ``` ```winsize(1:2) = [0 0]; ``` ```mov = moviein(numsteps+1,f,winsize); ``` ```mov(:,1) = getframe(f,winsize); ``` ```% Run through each timestep ``` ```for t = 0:dt:tf ``` ``` ``` ``` % update the true positions using the motor model ``` ``` [X(:,idx), Y(:,idx), Theta(:,idx), R(:,idx), Phi(:,idx), motorState] = ... ``` ``` motionModel(V(:,idx-1), W(:,idx-1), X(:,idx-1), Y(:,idx-1), Theta(:,idx-1), dt, motorState); ``` ``` %Phi ``` ``` ``` ``` % Update the sensor values using the sensor model ``` ``` [Xs(:,idx), Ys(:,idx), Thetas(:,idx), Phis(:,idx), sensorState] = ... ``` ``` sensorModel(X(:,idx), Y(:,idx), Theta(:,idx), Phi(:,idx), sensorState); ``` ``` %Phis ``` ``` % visualize real position ``` ``` mov = visualizeRobots(f,n,X,Y,Theta,V,idx,color,mov,winsize); ``` ``` ``` ``` ``` ``` if doFeedback == true ``` ``` [XD(:,idx), YD(:,idx), ThetaD(:,idx)] = desiredPosition(Xs(:,idx),Ys(:,idx),Phis(:,idx),desiredR,desiredPhi); ``` ``` ``` ```% ThetaD(:,idx) ``` ``` ``` ``` % don't compute for the queen ``` ``` [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)); ``` ``` end ``` ``` ``` ``` disp(t); ``` ``` ``` ``` %pause %(dt); ``` ``` ``` ``` idx = idx + 1; ``` ```end ``` ```if showPlots ``` ``` figure; ``` ``` hold on; ``` ``` title('true X vs. sensed X'); ``` ``` plot(1:idx-1, X(1,:),1:idx-1, Xs(1,:)) ``` ``` plot(1:idx-1, X(2,:),1:idx-1, Xs(2,:)) ``` ```end ``` ```if makeMovie ``` ``` disp('making movie...'); ``` ``` movie2avi(mov,'movie.avi'); ``` ``` disp('movie.avi created!'); ``` ```end ```