% PID control of a three pole process, h_p(s)=k(1-tau s)/((1+T1*s)(1+T2*s))
% with
% 1. The Skoestad method
% 2. Balchen parameters in the table at p. 344

clear all

T1=1; T2=0.5; tau=2; k=0.5;

A=[-1/T1  0
    1/T2 -1/T2];

B=[1/T1;0]
D=[-k*tau/T2, k+k*tau/T2];

ipi=3;
if ipi==1
% Skogestads metode
 T=T1+T2/2;
 tau=tau+T2/2

 Ti=T; Td=0;
 Kp=T/(2*k*tau);

 %Kp=1.3;
elseif ipi==2
% Balchen tabell s. 344, eks 5.3
 Ti=T1; Td=0;
 Kp=T1*(2*T2+tau)/(k*tau*(4*T2+tau));
 
elseif ipi==3  % Ziegler Nichols
    Kp=0.68; Ti=2.79; Td=0;
elseif ipi==4
    Kp=0.42; Ti=1; Td=0;  % Prøv og feil
    
end

dt=T2/10;
t=0:dt:20;
N=length(t);

x=[0;0]; z=0; yold=0;
r=1; 

for k=1:N
    y=D*x;
    
    e=r-y;
    u=z+Kp*e -Kp*Td*(y-yold)/dt;
    z=z+dt*Kp*e/Ti;
    yold=y;
    
    Y(k,1)=y; U(k,1)=u; R(k,1)=r;
    
    x=x+dt*(A*x+B*u);
end

figure(1)
plot(t,Y,'-',t,R,'-'), grid
title('Ex. PID control of process h_p=k/((1+T1s)(1+T2s)(1+T3s))')
xlabel('Time t [sec]'), ylabel('y')

figure(2)
plot(t,U), grid
title('Ex. PID control of process h_p=k/((1+T1s)(1+T2s)(1+T3s))')
xlabel('Time t [sec]'), ylabel('u')