Colony

% 4 robots

% Robot #1 is the queen

n = 4;

dt = 0.05;

tf = 10;

X = zeros(n,1); %cm

Y = zeros(n,1); %cm

R = zeros(n,1);

Theta = zeros(n,1); %rads

Phi = zeros(n,1); %rads

V = zeros(n,1); %0-255

W = zeros(n,1); %0-255

%W(1,1) = pi;

%X(2,1) = 2.3;

%Y(2,1) = 5.7;

for i=1:n

   X(i) = rand(1,1)*10;

   Y(i) = rand(1,1)*10;

   V(i) = rand(1,1)*10;

   W(i) = rand(1,1)*2*pi;

end

% The things that end in s are the sensor values

Xs = zeros(n,1);

Ys = zeros(n,1);

Thetas = zeros(n,1);

Phis = zeros(n,1);

% These state variables allow the models to maintain some state across calls

motorState = [];

sensorState = [];

% Run through each timestep

for t = 0:dt:tf

    % update the true positions using the motor model 

    [X, Y, Theta, R, Phi, motorState] = motionModel(V, W, X, Y, Theta, dt, motorState);

    %Phi

    % Update the sensor values using the sensor model

    [Xs, Ys, Thetas, Phis, sensorState] = sensorModel(X, Y, Theta, Phi, sensorState);

    %Phis

    axis([0 20 0 20])

axis manual

   %  set( gca,'nextplot','add' );

   % for j = 1:n

   %     color = [rand rand rand];

   %     h = plot_arrow( X(j),Y(j),X(j)+cos(Theta(j)),Y(j)+sin(Theta(j)),...

   %         'color',color,'facecolor',color,'edgecolor',color );

   %     set( h,'linewidth',2 );

   % end

   % hold off;

     set( gca,'nextplot','add' );

    for j = 1:n

        color = [rand rand rand];

        h = plot_arrow( Xs(j),Ys(j),Xs(j)+cos(Thetas(j)),Ys(j)+sin(Thetas(j)),...

            'color',color,'facecolor',color,'edgecolor',color );

        set( h,'linewidth',2 );

    end

    hold off;

    %TODO: at some point, out controller will go here

end