% Solution of Example task1
% Simulation of feedback control system with PI controller and SIMC
% setting for Kp and Ti.

clear all

% Model parameters
K=1; T1=30; T2=1; tau_p=1;

% Transfer function model without time delay
s=tf('s');

hpm=K/((1+T1*s)*(1+T2*s)*(1+T2*s))

num=[0,0,0,K];
den=[30,61,32,1];
[Ac,Bc,Dc]=tf2ss(num,den)
Cc=Bc;

% SIMC setting for Kp and Ti
T=T1+0.5*T2; tau=tau_p+1.5*T2;
Kp=T/(2*K*tau); 
Ti=min(T,8*tau); 
% Kp=5; Ti=10;

% Simulation interval
t_final=120;
dt=0.01;
t=0:dt:t_final;
N=length(t);

nt=tau_p/dt;       % Size of array to implement time delay
Delay=zeros(nt,1); % arrays to make time delay, hold (nt) samples


u=1; v=0; r=1; z=0;
x=zeros(3,1);
for k=1:N
    yp=Dc*x;         % Mesurement before delay
       
    %%% Implementation of time delay, using array method.
    % programming of time delay for process model, 
    % hold (y) in array of length (nt) samples
    y=Delay(nt); % the delayed process output Y=y(i-nt)
    for id=nt:-1:2;
      Delay(id)=Delay(id-1);
    end
    Delay(1)=yp;
    %%% End implementation of time delay    
    
    if k> N/2; v=1; end  % Step change in disturbance
    
    % PI controller
    e=r-y;
    u=Kp*e+z;
    z=z+dt*Kp*e/Ti;
    % end PI controller
    
    Y(k,1)=y;
    U(k,1)=u;
    
    x=x+dt*(Ac*x+Bc*u+Cc*v);
end

figure(1)
plot(t,Y), grid
title('Controlled output: y')
xlabel('Continuous Time  0 \leq t \leq t_f')

figure(2)
plot(t,U), grid
title('Control variable: u')

% Compute margins
hc=Kp*(1+Ti*s)/(Ti*s);
hp=hpm*exp(-tau_p*s);
h0=hc*hp;
% [Gm,Pm,Wc,W180] = margin(h0); 