%     PhaseLockRobots.m
%     
%
%  Robots with velocity controllers should be synchronised
%
%  J. Lunze
%  10.7.2020          (from Vernetzte Systeme/Uncertain systems)
%  Version of 19.2.2021
%
echo off
close all
clear
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  Robot model
kP=0.5;
A=[0 1; 0 -kP];
B=[0; kP];
C=[1 0];
D=0;
Robot=ss(A, B, C, D);
%step(Robot);
N1=5;
%
Tend=20;
dt=0.1;
Time=0:dt:Tend;
kend=length(Time);
%   Velocity set-points
vbar(1)=1;
vbar(2)=1.4;
vbar(3)=0.4;
vbar(4)=2.1;
vbar(5)=1.33;
%   initial positions
%x0=20*rand(2*N1,1);
s0(1)=2;
s0(2)=-10;
s0(3)=-12;
s0(4)=0;
s0(5)=8;

%%   Autonomous robot movement
%
figure(1)
subplot(3,1,[1,2]);
Yautforced=step(Robot, Time);
for k1=1:N1
    Y=Yautforced*vbar(k1)+s0(k1);
    plot(Time, Y, 'b');
    hold on;
end
latexylabel('$$s_i$$');
rotateY;
latexxlabel('$$t$$ in s');
xticks([0 5 10 15 20 25 30]);
yticks([-20 -10 0 10 20 40 60]);
axis([0 Tend -15 25]);
latextitle('$${\tt PhaseLockRobots:}$$ Autonomous robots');




%%  coupled robots
% all-to-all couplings
figure(2)
subplot(3,1,[1,2]);
kgain=0.35;   % coupling strength
L=-ones(N1,N1)+N1*diag(ones(N1,1));
%  Overall system
Abar=kron(diag(ones(N1,1)), A)...
     -kgain/N1*kron(diag(ones(N1,1)),B)*L*kron(diag(ones(N1,1)),C);
Bbar=kron(vbar', B);
Cbar=kron(diag(ones(N1,1)), C);
Dbar=0;
CoupledRobots=ss(Abar, Bbar, Cbar, Dbar);
x0=[s0(1) vbar(1) s0(2) vbar(2) s0(3) vbar(3) s0(4) vbar(4) s0(5) vbar(5)];
[Yfree,Time,Xfree]=initial(CoupledRobots, x0, Time);
[Yforced,Time,Xforced]=step(CoupledRobots, Time);
Y=Yfree+Yforced;
X=Xfree+Xforced;
plot(Time, Y, 'b');
latexylabel('$$s_i$$');
rotateY;
latexxlabel('$$t$$ in s');
xticks([0 5 10 15 20 25 30]);
yticks([-20 -10 0 10 20 40 60]);
axis([0 Tend -15 25]);
latextitle('$${\tt PhaseLockRobots:}$$ Synchronising robots');



%%  Average model and deviation model
%
figure(3)
subplot(3,1,[1,2]);
vav=sum(vbar)/N1;
s0av=sum(s0)/N1;
% average model
Yav=s0av+vav*Time;
% deviation model
Adev=[0 1; -kP*kgain -kP];
Bdev=[0; kP];
Cdev=[1 0];
Ddev=0;
DeviationModel=ss(Adev, Bdev, Cdev, Ddev);
%  maximum deviation of s_i from sbar
Deltas=max(vbar-vav)*dcgain(DeviationModel);
%
plot(Time, Y, 'b');
hold on
plot(Time, Yav, 'r--');
k1=ceil(kend/2);
%plot(Time(k1:kend), Yav(k1:kend)+Deltas, 'k--', Time(k1:kend), Yav(k1:kend)-Deltas, 'k--');
Timepoints=[k1*dt Tend Tend k1*dt k1*dt];
Ypoints=[Yav(k1)-Deltas Yav(kend)-Deltas Yav(kend)+Deltas Yav(k1)+Deltas Yav(k1)-Deltas];
fill(Timepoints, Ypoints, 'r');
latexylabel('$$s_i$$');
rotateY;
latexxlabel('$$t$$ in s');
xticks([0 5 10 15 20 25 30]);
yticks([-20 0 20 40 60]);
axis([0 Tend -15 25]);
latextitle('$${\tt PhaseLockRobots:}$$ Synchronising robots');
hold off


%%   Individual deviation of the robot behaviour from the average robot
%
figure(4)
plot(Time, Y(:,1), 'b');    % Position of Robot 1
hold on
x0dev=[x0(1)-s0av(1); x0(2)-vav];
Ydevfree=initial(DeviationModel, x0dev, Time);
Ydevforced=step(DeviationModel, Time)*(vbar(1)-vav);
Ydev=Ydevfree+Ydevforced;
plot(Time, Yav+Ydev, 'r--');
plot(Time, Yav, 'r');
hold off


%%  What happens if the robots are decoupled?
%
figure(5)
k2=ceil(3/5*kend);   % time, at which the agents are decoupled
Xinterm=X(k2,:);     % intermediate state of the robots
Vinterm=[Xinterm(2); Xinterm(4); Xinterm(6); Xinterm(8); Xinterm(10)];
Sinterm=[Xinterm(1); Xinterm(3); Xinterm(5); Xinterm(7); Xinterm(9)];
Time2=[0:dt:(kend-k2)*dt];
Yautforced=step(Robot, Time2);
Ynew=[];
for k1=1:N1
    Ynew(:,k1)=initial(Robot,[Sinterm(k1); Vinterm(k1)],Time2)...
           + Yautforced*vbar(k1);
end
Ydec=[Y(1:k2-1,:); Ynew];
plot(Time, Ydec, 'b');
hold on
plot([k2*dt k2*dt],[-20 30],'k:');
latexylabel('$$s_i$$');
rotateY;
latexxlabel('$$t$$ in s');
xticks([0 5 10 15 20 25 30]);
yticks([-20 -10 0 10 20 40 60]);
axis([0 Tend -15 25]);
latextitle('$${\tt PhaseLockRobots:}$$ Autonomous robots');



%%
%   EPS-figure
%
epsfigc15('PhaseLockRobots');






