Colony

% 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