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);


%%% 1st req. er<=10% R0 (step signal amplitude)
%er=1/(1+F0)<1/10
%F0=kpG0 -> 1+Kp*G0>10 (G0=1) -> kp>9

%%% 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) <=1 -> omega_nc>=omega_nc_th=3/zita_c_th=8.6

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

% set omega_nc=10

% set kp according to 1st req (kp>9)
kp=10;
F_pr=kp/(s^2+s+1);

% evaluate the margins, in particular the phase margin
figure
margin(F_pr)
hold on
grid on

% from asymptotic bode diagram -> overestimated phase margin and therefore zita_c
bodeas(F_pr)
hold on
legend ('asy.','real')

% zita_c from analytic results
zita_c=zita/sqrt(1+G0*kp);

% add a zero in the controller to increase the crossing frequency (omega_c)
% |F(j*omega_c)|=1, i.e 0 dB

zero_k=-1; % you get the desired amplification (20 dB) and an increase of the phase of 90 degrees in omega=10 
tau_z=-1/(zero_k);
Kz=(1+s*tau_z);
F_pr_z=kp*Kz*G;

figure
bode(F_pr,Kz,F_pr_z)
hold on
grid on
legend ('F_{pr}','zero','F_{pr,zero}')

% check the margins
figure
margin(F_pr_z);
grid on

% controller with a constant term (i.e. proportional action) and a zero at low frequency
%Kpd=kp*(1+s*tau_z); 
% it is like a PD controller:  PD= kp +kd*s= kp*(1+kd/kp*s)=kp*(1+Td*s), where Td=kd/kp


% to get a feasible controller -> add a pole at high frequencies
pole_k=-10; % or -20 (by adding a pole -20 you get omega_nc close to 10 and a greater m_ph) 
tau_p=-1/(pole_k);
Kp=1/(1+s*tau_p);

%Kpd_f=kp*(1+s*tau_z)/(1+s*tau_p);
Kpd_f=kp*Kz*Kp;

F_pr_z_p=kp*Kz*Kp*G;
figure
bode(F_pr,Kz,F_pr_z,Kp,F_pr_z_p)
hold on
grid on
legend ('F_{pr}','zero','F_{pr,zero}','pole','F_{pr,zero,pole}')


% check the margins
figure
margin(F_pr_z_p);
grid on

% define the closed loop system
Wpr=feedback(F_pr,1);  %  closed loop system with a controller including a proportional action (by kp)
Wpr_z=feedback(F_pr_z,1); %  closed loop system with a controller including a proportional action (by kp) and a zero (Kz)
Wpr_z_p=feedback(F_pr_z_p,1); %  closed loop system with a controller including a proportional action (by kp), a zero (Kz) 
                                                     % and a pole for the feasibility of the controller

figure
step(Wpr,Wpr_z,Wpr_z_p)
legend('prop.','prop-zero (PD)','prop-zero-pole (PD_r)')
grid on