% Problem 8.9
disp('Problem 8.9')
clear
A = [-0.58 0    0    -0.27  0     0.2    0
      0   -1    0     0     0     1      0
      0    0   -5     2.1   0     0      0
      0    0    0     0   337     0      0
     -0.14 0    0.14 -0.2  -0.28  0      0
      0    0    0     0     0     0.08   2
    -17.2 66.7 -11.6 40.9   0   -66.7  -16.7];
B = [0 0 0 0 0 0 1]';
C = [0 0 0 0 0 0 1];
D = 0;

% Transfer function of the system
[n,d]=ss2tf(A,B,C,D,1);
disp('(a) Uncompensated system:')
printsys(n,d,'s')

% Open-loop zeros and poles
disp('Open-loop zeros:')
z=roots(n)
disp('open-loop poles:')
p=roots(d)

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

% Steady state error
Kp=n(length(n))/d(length(d))
disp('Steady-state error:')
ess=1/(1+Kp)

disp('System with a PI compensator:')
nc=[1 0.1];
dc=[1 0.000];
n_c=-conv(nc,n);
d_c=conv(dc,d);
len_n=length(n_c);
len_d=length(d_c);
Kp_c=n_c(len_n)/d_c(len_d);

disp('Steady-state error of the compensated system:')
ess_c=1/(1+Kp_c)
subplot(222)
rlocus(n_c,d_c)
grid
title('(b) Compensated system')

disp('Closed-loop compensated system:')
[n_ccl,d_ccl]=cloop(n_c,d_c,-1);
printsys(n_ccl,d_ccl,'s')
disp('Poles of closed-loop compensated system:')
roots(d_ccl)

subplot(212)
step(n_ccl,d_ccl)
grid
xlabel('time [s]')
ylabel('yc')