%      Sprinkler.M
%   
%  Figures show the behaviour of an irrigation system
%  
% J. Lunze
% 
% 14.1.2019
% Version v. 14.1.2019
% for 2nd edition: 1.7.2021
%
echo off
clear
close all

%%     Parameters of the pillars
%  adapted from robot parameters in SyncIMPDemo.m
%
figure(1)
%               Vehicle models differ in the mass m
c=100;          %  c=200 kg/s friction constant
                % forces in N,  hence input  u=acceleration in N
                % time in seconds
                % distance in m
                % velocity in m/s,   
                %         hence output  y=velocity in m/s
m(1)=1000;      % m= 1000 kg  mass
m(2)=300;
m(3)=500;
m(4)=1800;      % command step response has an overshoot: 1.0077
m(5)=1200;
m(6)=300;
m(7)=2000;      % command step response has an overshoot: 1.012
m(8)=400;
m(9)=500;
m(10)=1200;
%
N1=10;           % N pillars
%
%               PI controller
kIm=30/500;     % kI=40 for nominal mass m=500;
kPm=450/500;          % kP=200 for nominal mass
%               Controlled vehicle
%               input = set-point for velocity controller
%               output = velocity
for k1=1:N1
    kI(k1)=kIm*m(k1); 
    kP(k1)=kPm*m(k1);
end
Ca=[1 0];
Da=0;
na=2;            % dynamical order of the controlled vehicle
r=1;
%      Draw the command step response of the velocity control loop 
%      for all vehicles
%      Determine the delay Delta(k1)
Tend=40;
dt=0.05;
Time=0:dt:Tend;
for k1=1:N1
    Aa=[-c/m(k1)-kP(k1)/m(k1) -kI(k1)/m(k1); 1 0];
    Ba=[kP(k1)/m(k1); -1];    
    Pillar=ss(Aa, Ba, Ca, Da);
    Y=step(Pillar,Time);
    subplot(4,1,[1 2])
    plot(Time, Y, 'b');
    hold on
    %  Determination of the delay
    Delta(k1)=trapz(1-Y)*dt;
end
latexxlabel('$$t$$ in s');
latexylabel('$$v_i$$ in $$\frac{\rm m}{\rm s}$$'); % in m/s
rotateY;
axis([0 20 0 1.2]);
yticks([0 1]);
xticks([0 5 10 15 20 30 40]);
latextitle('$${\tt Sprinkler:}$$ Command step response of the pillars');
hold off
%
nTS=3;            % pillar model extended by integrator to get the position
% initial states 
%Sprinkler1=rng;                         % save s to restore the generator setting
%save('Sprinkler1', 'Sprinkler1');
load('Sprinkler1');       % Seed (initialisation) of the number generator
rng(Sprinkler1);
s0=6*rand(N1,1);   % random positions
x0=zeros(N1*nTS,1);
for k1=1:N1
    x0((k1-1)*nTS+1,1)=s0(k1);
end


%%    Behaviour of the irrigation system with decentralised control
%  each pillar gets the command signal from the leader and reacts
%  independently of the others
%  the positions of the pillars show how the overall system gets deformed
%
figure(2)
Tend=40;
Time=0:dt:Tend;
vbar=0.8;  % velocity in m/s
Abar=zeros(N1*nTS, N1*nTS);
Gbar=zeros(N1*nTS,1);    % disturbance input
k1=5;                    % 5-th pillar gets the disturbance
Gi=[0; 1/m(k1); 0];      % for the disturbance input
Gbar((k1-1)*nTS+1:(k1-1)*nTS+3,1)=Gi;
k1=6;                    % 6-th pillar gets the disturbance
Gi=[0; 1/m(k1); 0];      % for the disturbance input
Gbar((k1-1)*nTS+1:(k1-1)*nTS+3,1)=Gi;
Bdiag=zeros(N1*nTS,N1);  % for the sync test (figure(9))
for k1=1:N1
    Aa=[-c/m(k1)-kP(k1)/m(k1) -kI(k1)/m(k1); 1 0];
    Ba=[kP(k1)/m(k1); -1];
    Ai=[0 1 0; zeros(2,1) Aa];
    Bi=[0; Ba];
    Abar((k1-1)*nTS+1:k1*nTS, (k1-1)*nTS+1:k1*nTS)=Ai;
    Bbar((k1-1)*nTS+1:k1*nTS, 1)=Bi;
    Bdiag((k1-1)*nTS+1:k1*nTS, k1)=Bi;
end
Cbar=kron(eye(N1,N1), [1 0 0]);
Dbar=0;
IrrigSyst=ss(Abar, Bbar, Cbar, Dbar);
IrrigSystDist=ss(Abar, [Bbar Gbar], Cbar, 0);
Y=step(IrrigSyst,Time);
%
subplot(4,1,[1,2]);
plot(Time, vbar*Y, 'b');
xleer;
xticksempty([0 10 20 30 40]);
latexylabel('$$s_i$$ in m');
rotateY;
yticks([0 10 20 30 40]);
axis([0 Tend 0 40]);
latextitle('$${\tt Sprinkler:}$$ Decentralised vs. networked control');


%%    Behaviour of the irrigation system with networked control
%
figure(2)
subplot(4,1,[3 4]);
AbarExt=Abar;
alpha=0.24;    % weighting factor for the two components of the controller
               %  cf. feedback gain for figure(5)
Ci=[1 0 0];
for k1=1:N1
    Ba=[kP(k1)/m(k1); -1];
    Bi=[0; Ba];
    AbarExt((k1-1)*nTS+1:k1*nTS, (k1-1)*nTS+1:k1*nTS) =...
            Abar((k1-1)*nTS+1:k1*nTS, (k1-1)*nTS+1:k1*nTS)...
            -alpha*Bi*Ci;
    if k1==1
        AbarExt(1:nTS,nTS+1:nTS+nTS)=alpha*Bi*Ci;
    end
    if k1==N1
        AbarExt((N1-1)*nTS+1:N1*nTS,(N1-2)*nTS+1:(N1-1)*nTS)=alpha*Bi*Ci;
    end        
    if k1~=1 && k1~=N1
        AbarExt((k1-1)*nTS+1:k1*nTS,(k1-2)*nTS+1:(k1-1)*nTS)=alpha/2*Bi*Ci;
        AbarExt((k1-1)*nTS+1:k1*nTS,k1*nTS+1:(k1+1)*nTS)=alpha/2*Bi*Ci;
    end
end
IrrigSystExt=ss(AbarExt, Bbar, Cbar, Dbar);
IrrigSystExtDist=ss(AbarExt, [Bbar Gbar], Cbar, 0);
YExt=step(IrrigSystExt,Time);
%
plot(Time, vbar*YExt, 'b');
latexxlabel('$$t$$ in s');
latexylabel('$$s_i$$ in m');
rotateY;
yticks([0 10 20 30 40]);
xticks([0 10 20 30 40]);
axis([0 Tend 0 40]);


%%  Positions of the irrigation system
%
figure(3)
subplot(4,1,[1 2]);
kend=length(Time);
for k1=1:N1
    Pos(k1)=Y(kend,k1);
end
plot([0:N1-1]*80, Pos, 'bo');
hold on;
plot([0:N1-1]*80, Pos, 'b');
latexylabel('$$s_i$$ in m');
rotateY;
xleer;
axis([-50 800 34 40]);
yticks([30 35 40]);
hold off
latextitle(['$${\tt Sprinkler:}$$ Position at time $$t=\; $' num2str(Tend) ]);
%
subplot(4,1,[3 4]);
for k1=1:N1
    PosExt(k1)=YExt(kend,k1);
end
plot([0:N1-1]*80, PosExt, 'bo');
hold on;
plot([0:N1-1]*80, PosExt, 'b');
latexylabel('$$s_i$$ in m');
rotateY;
latexxlabel('$$l_i$$ in m');
axis([-50 800 34 40]);
yticks([30 35 40]);
xticks([0 200 400 600 800]);
hold off


%%  Disturbance behaviour
%   5-th and 6-th pillar get additional force
%
figure(4)
Dist=[];
T0=0;
T1=10;
T2=20;
k0e=1; 
k1e=ceil((T1-T0)/dt);
k2e=k1e+ceil((T2-T1)/dt);
k3e=kend;
Dist=[0*ones(k1e-k0e,1); -200*ones(k2e-k1e,1); 0*ones(k3e-k2e+1,1)];
%
subplot(4,1,[1 2]);
%   inputs = [command input, disturbance]
YDist=lsim(IrrigSystDist, [vbar*ones(size(Dist)) Dist], Time);
%
fill([k1e*dt k2e*dt k2e*dt k1e*dt k1e*dt],[-0.2 -0.2 80 80 -0.2],...
        [0.9 0.9 0.9], 'EdgeColor', 'none');
hold on
plot(Time, YDist, 'b');
xleer;
latexylabel('$$s_i$$ in m');
rotateY;
yticks([0 10 20 30 40]);
xticksempty([0 10 20 30 40]);
axis([0 Tend 0 40]);
latextitle('$${\tt Sprinkler:}$$ Disturbance behaviour');
hold off

subplot(4,1,[3 4]);
%   inputs = [command input, disturbance]
YDist=lsim(IrrigSystExtDist, [vbar*ones(size(Dist)) Dist], Time);
%
fill([k1e*dt k2e*dt k2e*dt k1e*dt k1e*dt],[-0.2 -0.2 80 80 -0.2],...
        [0.9 0.9 0.9], 'EdgeColor', 'none');
hold on
plot(Time, YDist, 'b');
latexxlabel('$$t$$ in s');
latexylabel('$$s_i$$ in m');
rotateY;
yticks([0 10 20 30 40]);
xticks([0 10 20 30 40]);
axis([0 Tend 0 40]);
hold off

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Synchronisation of identical platforms
%
figure(5)
%     Average platform
mav=sum(m)/N1;
kIav=kIm*mav;
kPav=kPm*mav;
Aav=[0 1 0; 0 -c/mav-kPav/mav -kIav/mav; 0 1 0];
Bav=[0; kPav/mav; -1]; 
Cav=[1 0 0];
Dav=0;
nTS=3;
Platform=ss(Aav, Bav, Cav, Dav);
%
rlocprintblue(Platform);
latexxlabel('Re');
latexylabel('Im');
rotateY;
axis([-1.5 0.5 -1 1])
axis('square');
plot([-6 6],[0 0],'k:', [0 0],[-5 5],'k:');
latextitle('$${\tt Sprinkler:}$$ Root locus of the platform');
% rlocfind(Platform)   
kgain1=0.24;   % 0.48
Eig=rlocus(Platform,kgain1);
plot(real(Eig), imag(Eig), 'bd');
latextext(-1.2, 0.25, '$$k=0.24$$');
xticks([-2 -1 -0.5 0 0.5]);
yticks([-1 -0.5 0 0.5 1]);
hold off
%
%   Selection of the feedback gain
%   Input u_i to the platform is a velocity command signal
%   Consider 5 m position difference between neighbouring platforms
%   with 2 neighbours:  u_i = v_0 + 2 * 5 * kgain
%   the second term 2*5*kgain must not exceed 3*vbar=2.4
%   hence, kgain < 0.24
kgain=kgain1;

%%   Synchronisation behaviour
%
figure(6)
%
L=diag(2*ones(N1,1),0)-diag(ones(N1-1,1),-1)-diag(ones(N1-1,1),1);
L(1,1)=1;
L(N1,N1)=1;
%
AbarSyn=kron(diag(ones(N1,1)), Aav)-kron(kgain*L, Bav*Cav);
BbarSyn=kron(ones(N1,1), Bav);
CbarSyn=kron(diag(ones(N1,1)), Cav);
DbarSyn=zeros(N1,1);
SprinklerSys=ss(AbarSyn, BbarSyn, CbarSyn, DbarSyn);
Tend=20;
Time=0:dt:Tend;
Y=initial(SprinklerSys, x0, Time);
subplot(4,1,[1,2])
plot(Time, Y, 'b');
hold on
plot([0 Tend],[0 0],'k:');
axis([0 Tend 0 6]);
latexxlabel('$$t$$ in s');
%xleer;
latexylabel('$$s_i$$ in m');
rotateY;
latextitle('$${\tt Sprinkler:}$$ 10 platforms');
xticks([0 10 20 30]);
yticks([0 2 4 6]);
hold off


%%   Behaviour of a single platform
%    to show the meaning of the delay measure
%
figure(7)
%
Y=step(Platform,Time);
subplot(3,1,[1,2]);
plot(Time, vbar*Y, 'b');
hold on
plot([0 Tend],vbar*[0 Tend],'b--');
axis([0 6 0 5]);
xticks([0 2 4 6]);
yticks([0 2 4 6]);
latextitle('$${\tt Sprinkler:}$$ Step response of a platform');
latexxlabel('$$t$$ in s');
latexylabel('$$y_i$$ in m');
rotateY;
latextext(6.1, 4.5, '$$\bar{v} \Delta_i $$');
hold off


%%  Comparison of the movement of identical platforms without and with
%   communication links
%
figure(8)
AbarId=kron(diag(ones(N1,1)), Aav);
BbarId=kron(ones(N1,1), Bav);
CbarId=kron(diag(ones(N1,1)), Cav);
DbarId=zeros(N1,1);
SprinklerId=ss(AbarId, BbarId, CbarId, DbarId);
% initial states as before
Tend=40;
Time=0:dt:Tend;
Yfree=initial(SprinklerId, x0, Time);
Yforced=vbar*step(SprinklerId,Time);
subplot(4,1,[1,2])
plot(Time, Yfree+Yforced, 'b');
hold on
axis([0 Tend 0 40]);
xleer;
latexylabel('$$s_i$$ in m');
rotateY;
latextitle('$${\tt Sprinkler:}$$ Decentralised vs. networked control');
xticksempty([0 10 20 30 40]);
yticks([0 10 20 30 40]);
hold off
%
%  With communication links
Yfree=initial(SprinklerSys, x0, Time);
Yforced=vbar*step(SprinklerSys,Time);
subplot(4,1,[3,4])
plot(Time, Yfree+Yforced, 'b');
hold on
plot([0 Tend],[0 0],'k:');
axis([0 Tend 0 40]);
latexxlabel('$$t$$ in s');
latexylabel('$$s_i$$ in m');
rotateY;
xticks([0 10 20 30 40]);
yticks([0 10 20 30 40]);
hold off



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%   Synchronisation of pillars with individual dynamics
%
%figure(9)
%    Synchronisation test
Ap1=[-c/m(1)-kP(1)/m(1) -kI(1)/m(1); 1 0];
Bp1=[kP(1)/m(1); -1];
Aq1=[1 0; 0 0; 0 0];
%  Determination of Abreve
%  first addend: delete first rows, add new blocks
Abreve1=[Ap1 zeros(2,(N1-1)*nTS);...
         -kron(ones(N1-1,1),Aq1)  Abar(4:end, 4:end)];
%  second addend: 
Bbreve=[Bp1 zeros(2,N1-1);...
        zeros((N1-1)*nTS,1)  Bdiag(4:end, 2:end)];
Cbreve=[zeros(1,2) zeros(1,(N1-1)*nTS);...
        zeros(N1-1,2) Cbar(2:end,4:end)];
%   Summary
Abreve=Abreve1-Bbreve*kgain*L*Cbreve;   % test successful!

%%
%
epsfigc15('Sprinkler');
