% 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)
GpH = Gp;    % H = 1
% 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;     % 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 = K*Gp / (1 + K*GpH);
closed_loop_poles = pole (minreal (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 = K(i)*Gp / (1 + K(i)*GpH);
    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
figure('Name', 'step response comparison for all controllers');
step (Kg*Gp / (1 + Kg*GpH))
hold on
step (Gcg*Gp / (1 + Gcg*GpH))
hold on
step (Gcd*Gp / (1 + Gcd*GpH))
legend ('gain', 'phase-lag', 'phase-lead');
hold off

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