% Problem 8.18
disp('Problem 8.18 - Phase-lag-lead controller')
clear
n=[0 0 0 0 0 1];
d1=conv([1 0.2],[1 0.5]);
d2=conv([1 10],conv([1 14.28],[1 25]));
d=conv(d1,d2);
disp('Original system')
K=154280;
printsys(K*n,d,'s')

subplot(221)
rlocus(n,d)
axis([-30 5 -10 10])
title('(a) Uncompensated system')
grid

% Operating point
OS=0.05; ts=3;
xi=sqrt(log(OS)^2/(pi^2+log(OS)^2));
xi=xi+0.005 % to reduce overshoot
om_n=3/(xi*ts)
disp(' '); disp('Operating point:')
sd=-xi*om_n+om_n*sqrt(1-xi^2)*i

% Phase-lead part of controller
%  - phase contribution
th_p1=angle(sd+0.2);
th_p2=angle(sd+0.5);
th_p3=angle(sd+10);
th_p4=angle(sd+14.28);
th_p5=angle(sd+25);

th_c=pi+th_p1+th_p2+th_p3+th_p4+th_p5
while th_c - 2*pi > 0,
      th_c=th_c-2*pi;
end
disp('Controller phase contribution [rad]:')
th_c

% Choose zc=0.7 and compute pc
zc=0.67;
phi=angle(sd+zc)
pc=xi*om_n+om_n*sqrt(1-xi^2)*tan(th_c-phi+pi/2)
nc_lead=[1 zc];
dc_lead=[1 pc];
disp('Phase-lead compensator:')
printsys(nc_lead,dc_lead,'s')

% Static gain K
dp1=abs(sd+0.2)
dp2=abs(sd+0.5)
dp3=abs(sd+10)
dp4=abs(sd+14.28)
dp5=abs(sd+25)
dpc=abs(sd+pc)
dzc=abs(sd+zc)
disp('Static gain at the operating point:')
Kstat=dp1*dp2*dp3*dp4*dp5*dpc/dzc

num_c=conv(n,nc_lead);
den_c=conv(d,dc_lead);

% Error constant
Kp=Kstat*num_c(length(num_c))/den_c(length(den_c));
ess=1/(1+Kp);

% Phase-lag part of the controller
nc_lag=[1 0.1];
%dc_lag=[1 0.006];
dc_lag=[1 0.01];
n_c=conv(Kstat*num_c,nc_lag);
d_c=conv(den_c,dc_lag);
len_n=length(n_c);
len_d=length(d_c);

% Steady state error of the system with the phase-lead compensator
Kp_c=n_c(len_n)/d_c(len_d)
ess=1/(1+Kp)
while (Kp_c-99) < 0,
   dc_lag(2)=0.9*dc_lag(2);
   d_c=conv(den_c,dc_lag);
   Kp_c=n_c(len_n)/d_c(len_d);
end

disp('Phase-Lag Compensator:')
printsys(nc_lag,dc_lag,'s')

disp('Steady-state error of the compensated system:')
ess_c=1/(1+Kp_c)

subplot(222)
rlocus(n_c,d_c)
axis([-30 5 -10 10])
title('(b) Compensated system')
grid

[nc_cl,dc_cl]=cloop(n_c,d_c,-1);

% Step response of the compensated system
t=0:0.1:30;
[yc,xc]=step(nc_cl,dc_cl,t);
subplot(212)
plot(t,yc)
xlabel('(c) time [s]')
ylabel('yc')
grid
