Colony

% n robots

% Robot #1 is the queen

n = 13;

%for movie output use:

 dt = 0.041709;

%else:

%dt = .01;

tf = 5.5 %40;

%shape numbers:

%0 = nothing

%1 = circle

%2 = square

%3 = diamond

%4 = arrow

%5 = semicircles

numsteps = ceil(tf / dt) + 1;

%%% output options

makeMovie = false;

showPlots = true;

doFeedback = true;

shape = 4;

if shape == 1

    n = 65;

elseif shape < 4

    n = 65;

elseif shape == 4

    n = 65;

elseif shape == 5

    n = 20;

end

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

    if shape == 1

    %circle

    desiredR(:) = 20;

    desiredPhi = (0:(n-1))*(2*pi/(n-1));

    elseif shape == 2

    %square

    desiredPhi = (0:(n-1))*(2*pi/(n-1))+pi/2;

    for index=2:(n-1)/4

        desiredR(index) = 20/cos(abs(desiredPhi(index)-3*pi/4));

    end

    for index=1+(n-1)/4:(n-1)/2

        desiredR(index) = 20/sin(abs(desiredPhi(index)-3*pi/4));

    end

    for index=1+(n-1)/2:3*(n-1)/4

        desiredR(index) = 20/sin(abs(desiredPhi(index)+3*pi/4));

    end

    for index=1+3*(n-1)/4:n-1

        desiredR(index) = abs(20/sin(abs(desiredPhi(index)-3*pi/4)));

    end

    desiredR(n) = 20*sqrt(2);

    desiredPhi = desiredPhi-pi/4;

    elseif shape == 3

    %diamond

    desiredPhi = (0:(n-1))*(2*pi/(n-1))+pi/2;

    for index=2:(n-1)/4

        desiredR(index) = 20/cos(abs(desiredPhi(index)-3*pi/4));

    end

    for index=1+(n-1)/4:(n-1)/2

        desiredR(index) = 20/sin(abs(desiredPhi(index)-3*pi/4));

    end

    for index=1+(n-1)/2:3*(n-1)/4

        desiredR(index) = 20/sin(abs(desiredPhi(index)+3*pi/4));

    end

    for index=1+3*(n-1)/4:n-1

        desiredR(index) = abs(20/sin(abs(desiredPhi(index)-3*pi/4)));

    end

    desiredR(n) = 20*sqrt(2);

    elseif shape == 4

    %arrow

    desiredPhi(2:(n-1)/4) = pi;

    desiredPhi(1+(n-1)/4:(n-1)/2) = 0;

    for index=1+(n-1)/2:n

        desiredPhi(index) = index*(2*pi/(n-1))+pi/2;

    end

    for index=2:(n-1)/4

        desiredR(index) = index*80*sqrt(2)/(n-1);

        desiredR(index+(n-1)/4) = desiredR(index);

    end

    for index=1+(n-1)/2:3*(n-1)/4

        desiredR(index) = 20/sin(abs(desiredPhi(index)+3*pi/4));

    end

    for index=1+3*(n-1)/4:n-1

        desiredR(index) = abs(20/sin(abs(desiredPhi(index)-3*pi/4)));

    end

    elseif shape == 5

    %2 circles

    desiredR(1:(n/2)) = 20;

    desiredR((n/2)+1:n) = 40;

    desiredPhi(1:(n/2)) = (1:(n/2))*(2*pi/(n-1));

    desiredPhi((n/2)+1:n) =(1:(n/2))*(2*pi/(n-1))+pi/3;

    end

    V(1,end/2:3*end/4) = 1;

    %changing orientation of the queen

    W(1,floor(end/6):floor(end/6)+20) = pi/2;

    W(1,floor(end/3):floor(end/3)+20) = -1*pi/2;

    % 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 = hsv(n);

encoderNoise = .01 * randn(2, 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 = [];

%Running error count for integral control

XYError = zeros(n,numsteps);

ThetaError = zeros(n,numsteps);

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);

% init

xoldSensor = 0;

yoldSensor = 0;

thetaoldSensor = 0;

% Run through each timestep

for t = 0:dt:tf

    % update the true positions using the motor model 

    [X(:,idx), Y(:,idx), Theta(:,idx),  Phi(:,idx), motorState, wheels] = ...

        motionModel(V(:,idx-1), W(:,idx-1), X(:,idx-1), Y(:,idx-1), Theta(:,idx-1), dt, motorState, n);

    %Phi

    % Update the sensor values using the sensor model

    [Xs(:,idx), Ys(:,idx), Thetas(:,idx), Phis(:,idx), sensorState, encoderNoise, xoldSensor, yoldSensor, thetaoldSensor] = ...

        sensorModel(X(:,idx), Y(:,idx), Theta(:,idx), Phi(:,idx), sensorState, n, encoderNoise, wheels, xoldSensor, yoldSensor, thetaoldSensor, dt);

    %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),XYError(2:end,idx),ThetaError(2:end,idx),V(2:end,idx-1),W(2:end,idx-1));

    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