% Problem 8.16
disp('Problem 8.16 - Phase-lead compensator')
clear
n=[0 0 1 10];
d=conv([1 20],[1 2 2]);
disp('Original system (K=1)')
printsys(n,d,'s')
subplot(221)
rlocus(n,d)
axis([-40 5 -30 30])
title('(a) Uncompensated system')
grid

% Operating point
OS=0.2; ts=1.5
xi=sqrt(log(OS)^2/(pi^2+log(OS)^2)); 
xi=xi+0.005; % to reduce overshoot
om_n=3/(ts*xi)

disp(' ')
disp('Operating point:')
sd=-xi*om_n + om_n*sqrt(1-xi^2)*i

% Controller phase contribution
[p,z]=pzmap(n,d);
th_z1=angle(sd-z(1));
th_p1=angle(sd-p(1));
th_p2=angle(sd-p(2));
th_p3=angle(sd-p(3));

th_c=pi-th_z1+th_p1+th_p2+th_p3;

while th_c - 2*pi > 0,
      th_c=th_c-2*pi;
end

disp('Controller phase contribution [rad]:')
th_c

% Choose zc=12 and compute pc
zc=12;
phi=angle(sd+zc)
pc=xi*om_n+om_n*sqrt(1-xi^2)*tan(th_c-phi+pi/2);

nc=[1 zc];
dc=[1 pc];
disp('Phase-lead compensator:')
printsys(nc,dc,'s')

num_c=conv(n,nc);
den_c=conv(d,dc);
subplot(222)
rlocus(num_c,den_c)
axis([-40 5 -30 30])
title('(b) Compensated system')
grid

% Static gain K
dp1=abs(sd-p(1))
dp2=abs(sd-p(2))
dp3=abs(sd-p(3))
dpc=abs(sd+pc)
dz1=abs(sd-z(1))
dzc=abs(sd+zc)

disp('Static gain:')
K=dp1*dp2*dp3*dpc/(dz1*dzc)

% Step responses
[n_cl,d_cl]=cloop(K*n,d,-1);
[nc_cl,dc_cl]=cloop(K*num_c,den_c,-1);

disp('Closed-loop poles:')
roots(dc_cl)

t=0:0.05:5;
[y,x]=step(n_cl,d_cl,t);
[yc,xc]=step(nc_cl,dc_cl,t);
subplot(212)
plot(t,y,'--',t,yc,'-')
xlabel('time [s]')
ylabel('y, yc')
grid

