% Problem 8.14
disp('Problem 8.14')
clear
n=[1 6];
d=conv([1 10],[1 2 2]);

% Open-loop poles
disp('open-loop poles:')
p=roots(d);

% Operating point sd
OS=0.2; ts=1.5;
xi=sqrt(log(OS)^2/(log(OS)^2+pi^2))+0.004
om_n=3/(ts*xi);
sd=-xi*om_n + i*om_n*sqrt(1-xi^2)

dp1=abs(sd-p(1));
dp2=abs(sd-p(2));
dp3=abs(sd-p(3));
dz1=abs(sd+6);

% Phase contribution
th_z1=angle(sd+6)
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

% Zeros and poles of a phase-lead compensator
for j=1:15,
  zc(j)=j;
  phi=angle(sd+zc(j));
  pc(j)=xi*om_n+om_n*sqrt(1-xi^2)*tan(th_c-phi+pi/2);

  nc=[1 zc(j)];
  dc=[1 pc(j)];

% System with a phase-lead compensator
  n_c=conv(nc,n);
  d_c=conv(dc,d);

% Static gain of the compensated system
  dpc=abs(sd+pc(j));
  dzc=abs(sd+zc(j));
  Kc=dp1*dp2*dp3*dpc/(dz1*dzc);

% Closed-loop compensated system
  [n_ccl,d_ccl]=cloop(Kc*n_c,d_c,-1);

% Steady state constant
  Kp=n_ccl(length(n_ccl))/d_ccl(length(d_ccl));

% Steady state error
  ess(j)=1/(1+Kp);
end
subplot(211)
plot(zc,ess,'x')
grid
xlabel('zc')
ylabel('e_ss')
title('Steady state error')

subplot(212)
plot(zc,pc,'o')
grid
xlabel('zc')

ylabel('pc')