% Problem 8.19
disp('Problem 8.19 - Phase-lead controller')
clear
n=[0 0 0 1];
d=conv([1 0],conv([1 0.0546],[1 1.55]));
disp('Original system')
K=0.84824;
printsys(K*n,d,'s')

subplot(221)
rlocus(n,d);
axis([-2 0.5 -2 2])
title('(a) Uncompensated system')
grid

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

% Phase-lead part of a controller
%  - phase contribution
th_p1=angle(sd)
th_p2=angle(sd+0.0546)
th_p3=angle(sd+1.55)

th_c=pi+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=0.045 and compute pc
zc=0.045;
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)
dp2=abs(sd+0.0546)
dp3=abs(sd+1.55)
dpc=abs(sd+pc)
dzc=abs(sd+zc)

disp('Static gain at the operating point:')
Kstat=dp1*dp2*dp3*dpc/dzc

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

subplot(222)
rlocus(num_c,den_c)
axis([-2 0.5 -2 2])
title('(b) Compensated system')
grid

[nc_cl,dc_cl]=cloop(Kstat*num_c,den_c,-1);

% Step response of the compensated system
t=0:0.1:30;
[yc,xc]=step(nc_cl,dc_cl,t);
subplot(223)
plot(t,yc,'-')
xlabel('(c) time [s]')
ylabel('yc')
grid
subplot(224)
plot(t,yc,'-')
xlabel('(d) time [s]')
ylabel('yc')
axis([5 10 0.9 1.1])
grid