clear all
clc
close all

% For the process P=0.8/(s^2+4.4*s+1.6), direct design of a digital control D(z) by Root Locus 
% in the z-plane in order to satisfy the following requirements

%%%% REQUIREMENTS %%%%%%%%%%%%%%%%%%
% - error at steady state <=0.1 wrt to a step reference signal with
% amplitude R0=1
% - s% <= 15% 
% - ta5% <= 0.6 sec 
% - Ts = 0.1 sec 

% define the plant to be controlled
s=tf('s');
Gs=0.8/(s^2+4.4*s+1.6);

% or using tf function
Gs1=tf(.8,[1 4.4 1.6]);


% ZOH method to find the z-trasform G(z) 
Ts=0.1;
Gz=c2d(Gs,Ts);

num_Gz=cell2mat(Gz.Numerator);
den_Gz=cell2mat(Gz.Denominator);

% zero(z)-pole(p)-gain(k) form of G(z)
[zero_Gz,poles_Gz,k_G] = tf2zpk(num_Gz,den_Gz);

z1=zero_Gz; p1=poles_Gz(1); p2=poles_Gz(2);
z=tf('z');

% Gz=k_G*(z-z1)/(z-p1)/(z-p2)
%Gz_s=(z-z1)/(z-p1)/(z-p2);
Gz_s=Gz/k_G;

%%%%%%%%%%%%%%%%%%%%%%%%%
%%% in ordet to satisfy the requirements %%%%
%%%%%%%%%%%%%%%%%%%%%%%%%

% error at steady state (err_ss) <=0.1 wrt a step signal with amplitude
% R0=1 ==> err_ss= R0/(1+F(1))<=0.1; 
% in the case F=kp*Gz==> err_ss=R0/(1+kp*Gz(1))<=0.1. As Gz(1)=1/2
% ==> 1+kp/2>=10 ==> kp>=9*2=18

% settling time ta5 <=0.6 sec;
% in s-domain, ta5=3/sigma, where sigma is the real part of the poles with change of sign   
% sigma=zita*omega_n, with sigma>0  
% ==> sigma>=5 (sigma_bar=5)
% then -sigma <= -sigma_bar; 
% i.e. the real part of the poles in s-domain has to be <= -sigma_bar (the threshold value for
% sigma)

%  in z-domain, this constraint on ta5 is expressed by r<=r_bar, 
%  with r the radius of the poles in z-plane being less than or
%  equal to r_bar (=exp(-sigma_bar*Ts))
sigma_bar=5;
r_bar=exp(-sigma_bar*Ts);


% overshoot less or equal to 15% =>  zita>= 0.5 => closed loop poles inside the
% relative curve zita= 0.5
% plot the root locus of the open loop function F1 with K(z)=1:  F1=(z-z1)/(z-p1)/z(-p2), i.e. Gz/k_G
F1=Gz_s;
figure
rlocus(F1);
hold on
grid on

xlim([-1.5 1.5])
ylim([-1.5 1.5])

% introduce a zero-pole in order to get the closed-loop poles inside the desired region
K2=(z-0.1)/(z+0.6);

F2=K2*Gz_s;
figure
rlocus(F2);
hold on
grid on

% check the locus
% rho= rho_k*k_G, 
% remind the static gain of the controller (i.e. Kz(1)) has to be >=18 for the error requirement
% Kz(1)=rho_k*dcgain(K2) where dcgain(K2) is the designed K2 evaluated at z=1, K2(z=1)
% Kz(1)>=18, then rho_k>=18/dcgain(K2) and rho>=18/dcgain(K2)*k_G

rho_min=18/dcgain(K2)*k_G;
rho_k_min=rho_min/k_G; %i.e 18/dcgain(K2)
% check the locus, for rho=rho_min, if the poles are inside the desired region

Kz=18/dcgain(K2)*K2;
Fz=Kz*Gz;

% define the closed loop system (Wz) 
Wz=feedback(Fz,1);

% plot the step response of Wz
figure
hold on
grid on
step(Wz)

% open the relative simulink block diagram
scheme_exam_IA_3_July_2024