clear all
close 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. omega_nc about 2 rad/s

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


ki_1=1;
K_i=ki_1/s;

% open loop function with the only integral action
F_i1=K_i/(s^2+s+1);
figure
bode(G,K_i,F_i1);
grid on
hold on
legend

figure
% evaluate the margin
margin(F_i1)
grid on

figure
% reduce the gain for getting a closed-loop system stable
ki_2=1/2;
K_i_2=ki_2/s;

F_i2=K_i_2*G;
margin(F_i2)
grid on

% compare the real bode diagrams with the asym. ones
bodeas(F_i2)
% to get omega_c=2 it is needed to increase the magnitude by 20 dB and
% to increase the phase at omega_c by (27+35) degrees (looking at the
% asymptotic diagrams, indeed arg(F_i2(j2))=-(180+27) degree)
% 
% add a zero for increasing the crossing frequency
zero_k=-0.2; % we get the desired amplification in omega=2
tau_z=-1/(zero_k);
K_z=(1+s*tau_z);   
 
K_pi= K_i_2*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_2) and a zero (K_z) 
F_pi=K_pi*G;

% plot the Bode diagrams
figure
bode(F_i2,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

% define the close-loop system for the different configurations
W_i1=feedback(F_i1,1); % closed loop system with a controller including an integrator (K_i_1) 
W_i2=feedback(F_i2,1); % closed loop system with a controller including an integrator (K_i_2) 
W_pi=feedback(F_pi,1); % closed loop system with a controller including an integrator (K_i_2) and a zero (K_z) 

% plot the step function for the different closed loop systems
figure
step(W_i1),
hold on
grid on
legend ('I_{cr.}')
figure
step(W_i2, W_pi)
legend ('I_{st.}','PI')
grid on