% ch9ex.m
% based on servomotor-based radar tracking system from Section 2.7.2
% used as example throughout Chapter 9

format compact
clc
clear

% Uncompensated open loop function
% G = GpH = 4 / (s*(s+1)*(s+2)) = 4 / (s^3 + 3*s^2 + 2*s)
% zeros: none   poles: 0, -1, -2
N = [4];
D = [1 3 2 0];
Gp = tf (N, D)
H = 1
GpH = Gp;
% alternative: Gp = GpH = zpk([], [0,-1,-2], 4)

% Root Locus
% breakaway points:
p1 = conv (N, polyder(D));
p2 = conv (polyder(N), D);
test = [0 p1] - p2;     % "[0 " necessary to make polynomials same size
breakaway = roots (test);
disp ('breakaway point:')
s = breakaway (2)
K = -polyval(D, s) / polyval(N, s)
% from Routh-Hurwitz stability criterion or j-axis crossing:  K < 1.5 
rlocus (GpH)

% gain compensation
disp (' ');  disp ('gain compensation')
disp ('stability condition: K < 1.5')
K = input ('Enter K: ');
figure ('Name', ['K = ' num2str(K)]); bode (K*GpH)
[gm,pm,wg,wp] = margin (K*GpH); gm, pm
gm_dB = 20 * log10(gm)
figure ('Name', ['K = ' num2str(K)]); nyquist (K*GpH, {.5, 1000})

% closed loop poles and time constants
% T = minreal (K*Gp / (1 + K*GpH));
T = feedback (K*Gp, H);
closed_loop_poles = pole (T)
time_constants = -1 ./ real(closed_loop_poles)
figure('Name', ['step response with K = ' num2str(K)]);
step (T)

% step response for various K values
figure('Name', 'step response for various Ks');
K = [.1 .25 .5];  t = 0:0.1:20;
for i = 1:length(K)
    T = feedback (K(i)*Gp, H);
    step (T, t)
    hold on;    % add to existing figure
end
legend (['K = ' num2str(K(1))], ['K = ' num2str(K(2))], ['K = ' num2str(K(3))]);
hold off;

% % gain compensator with a phase margin of 50 degrees
% disp (' ');  disp ('gain compensation for 50 degree phase margin:')
% w = roots ([-1 3*tan(130*pi/180) 2]);
% wc = w(2);
% Kg = 1 / abs(evalfr (GpH, j*wc))
% [gm,pm,wg,wp] = margin (Kg*GpH); gm, pm
% gm_dB = 20 * log10(gm)
% 
% % phase-lag compensator
% disp (' ');  disp ('phase-lag compensation')
% Kc = Kg;
% w = roots ([-1 3*tan(125*pi/180) 2]);
% w1 = w(2);
% w0 = 0.1 * w1
% wp = w0 / abs(evalfr (Kc*GpH, j*w1))
% Gcg = Kc * tf([1/w0 1], [1/wp 1])
% [gm,pm,wg,wp] = margin (Gcg*GpH); gm, pm
% gm_dB = 20 * log10(gm)
% 
% % phase-lead compensator
% disp (' ');  disp ('phase-lag compensation')
% % a0 = Kg
% a0 = 1
% Ts = 4
% pm = 50*pi/180;
% w1 = 8 / (Ts * tan(pm));
% theta = -pi + pm - angle(evalfr (GpH, j*w1));
% GpHw1 = abs (evalfr (GpH, j*w1));
% a1 = (1 - a0*GpHw1*cos(theta)) / (w1*GpHw1*sin(theta))
% b1 = (cos(theta) - a0*GpHw1) / (w1 * sin(theta))
% Gcd = tf([a1 a0], [b1 1])
% [gm,pm,wg,wp] = margin (Gcd*GpH); gm, pm
% gm_dB = 20 * log10(gm)
% 
% % compare all compensators
% t = 0:0.1:20;
% figure('Name', 'step response comparison for all controllers');
% step (Kg*Gp / (1 + Kg*GpH), t)
% hold on
% step (Gcg*Gp / (1 + Gcg*GpH), t)
% hold on
% step (Gcd*Gp / (1 + Gcd*GpH), t)
% legend ('gain', 'phase-lag', 'phase-lead');
% hold off

% pause for the user and remove figures
display ('Hit Enter to continue');
pause
close all
