close all
clear all

s=tf('s');

% plant to be controlled

% define the tf by G
G=1/(s^2+s+1);  % (s^2/omega_n^2 +2*zita/omega_n*s+1)

% or define the coeffs of N(s) and D(s) (G(s)=N(s)/D(s)) and then 
% define the object sys by tf
num=1;
den=[1 1 1];
sys=tf(num,den);

% compute the static gain, 
G0=dcgain(G);

% define omega_n and zita of the plant G
omega_n=1;
zita=1/2;
% or compute omega_n and zita by using damp function 
[Wn,Z] = damp(G);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%% Requirements of the control system %%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% 1st req.,  er=0 -> integrator, i.e. integral action, ki/s,  pole in the origin (1/s), gain ki

% 2nd req. s<=30% 
% zita_c>zita_c_th=0.35  s=exp(-pi*zita_c/sqrt(1-zita_c^2)), zita_c and m_ph (phase margin of F)
%-> m_ph=zita_c*100 -> m_ph>35 degrees

% 3rd req.  ta5% =3/(zita_c*omega_nc) <=2 ->
%omega_nc>=omega_nc_th=3/zita_c_th/2=4.3

% omega_nc=5 
%  omega_nc is approximated by the crossing frequency (omega_c) of F (open
%  loop function)
% |F(j*omega_c)|=1, i.e 0 dB

% fix ki
ki=1;
K_i=ki/s;

% open loop function with the only integral action
F_i=K_i*G;

% plot the Bode diagrams
figure(1)
bode(G,K_i,F_i);
grid on
hold on
legend

% compute the margins
figure
margin(F_i)
grid on

% compare the real bode diagrams with the asym. ones
bodeas(F_i)
hold on
legend ('asy.','real')

% in order to satisfy the 3rd requirement (omega_c=5) it is needed to
% increase the magnitude of F in omega_c by 42 dB , indeed the magnitude of
% |F_i(j5)|dB=-42; in order to satisfy the 2nd req. (m_ph> 35), it is
% needed to increase the phase of F_i in omega_c at least by 95/100 degrees (so it is needed two zeros),
% indeed arg(F_i(j5))=-240 (value from the asymptotic diagrams, note that this
% value is oveestimated, the phase value in 5 is lower, see the real Bode diagrams) 

% add a zero for increasing the crossing frequency...but not able to
% satisfy the desired m_ph (2nd requirement)

zero_k=-0.05; % with this zero the new omega_c is about 5  

tau_z=-1/(zero_k);
K_z=(1+s*tau_z);   
 
K_pi= K_i*K_z; % ki/s*(1+s*tau_z), it works as  PI controller: kp+ki/s= ki/s(1+kp/ki*s)=ki/s*(1+Ti*s)  Ti=kp/ki; 

% open loop function with a controller including an integrator (K_i) and a zero (K_z) 
F_pi=K_pi*G;


% plot the Bode diagrams
figure
bode(F_i,K_pi,F_pi);
hold on
grid on
legend ('F_{i}','PI','F_{PI}')
% check the margins
figure
margin(F_pi)
grid on

% compare the real bode diagrams with the asym. ones
bodeas(F_pi)
hold on
legend ('asy.','real')

% define the closed-loop system for the different configurations
W_i=feedback(F_i,1); % controller including only integrator
W_pi=feedback(F_pi,1); % controller including an integrator and a zero

% plot the step function
figure
step(W_i),
legend ('I_{cr}')
figure
step( W_pi)
legend ('PI')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% it is needed a controller with two zeros and one pole for feasibility %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% add a first zero at low frequency
zero_k1=-0.1; % we get an amplification of 20 dB + 14 dB (=log10(5)*20) in omega=5  
tau_z1=-1/(zero_k1);
K_z1=(1+s*tau_z1);   
 
K_pi1= K_i*K_z1; 
F_pi1=K_pi1*G;

figure
margin(F_pi1);
hold on
grid on
legend ('F_{PI1}')

% add a second zero around the crossing frequency (|z2|< omega_c=5) and 
% then a pole (|p|>omega_c=5)  for making the controller feasible 
zero_k2=-2; % by adding this zero we get a further amplification in |F(j5)|, that is (log10(5)-log10(2))*20, about 8 dB
tau_z2=-1/(zero_k2);
K_z2=(1+s*tau_z2);   
F_pi1_z2=F_pi1*K_z2;

figure
bode(F_pi1,K_z2,F_pi1_z2);
hold on
grid on
legend ('F_{PI1}','F_{PI1-z2}')

figure
margin(F_pi1_z2)
grid on
legend ('F_{PI1-z2}')

% compare the real bode diagrams with the asym. ones
bodeas(F_pi1_z2)
legend ('F_{PI1-z2}')


hold on

% add a pole in -10 or - 20 (for making the controller feasable)
% by adding the pole after omega_c=5 you don't affect |F(j5)|...you can
% reduce the phase lead: with the two zeros you get an increase of +90 degrees (due
% to z1) and an additional one of +18 degrees (i.e. (log10(5)-log10(2))*45=17.9) due to z2; 
% see the Bode diagrams for the lead compensator (z2 and p)

pole_k=-10; %   
tau_p=-1/(pole_k);
K_pol=1/(1+s*tau_p);
Kz2_pol=K_z2*K_pol;
bodeas(Kz2_pol)
title('lead compensator (zero-pole)')
F_pi1_z2_r=F_pi1*Kz2_pol;

% compare the real bode diagrams with the asym. ones
bodeas(F_pi1_z2_r)
hold on
legend ('F_{PI1-z2_r}')

figure
margin(F_pi1_z2_r)
grid on
legend ('F_{PI1-z2-r}')


% define the closed loop system for the different configurations
W_pi1=feedback(F_pi1,1);
W_pi_z2=feedback(F_pi1_z2,1);
W_pi_z2_r=feedback(F_pi1_z2_r,1);

% plot the step function
figure
step(W_pi1,W_pi_z2,W_pi_z2_r),
legend ('PI1','{PI1-z2}','{PI1-z2-r}')
grid on