% mypid.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
%figure ('Name', 'OLF Bode'); bode (GpH)
%figure ('Name', 'OLF Nyquist'); nyquist (GpH, {.5, 1000})

% P controller with 50 deg phase margin
disp (' ');  disp ('P controller')
w = roots ([-1 3*tan(130*pi/180) 2]);
wc = w(2);
Kp = 1 / abs(evalfr (GpH, j*wc))
Gcp = Kp;
[gm,pm,wg,wp] = margin (Kp*GpH); pm
%figure ('Name', 'P Bode'); bode (Gc*GpH)
%figure ('Name', 'P Nyquist'); nyquist (Gc*GpH, {.5, 1000})

% PI controller with 50 deg phase margin
disp (' ');  disp ('PI controller')
%Kc = Kp;
Kc = 1
w1 = w(2);
Kp = 1 / abs(evalfr (Kc*GpH, j*w1))
w0 = 0.1 * w1;
Ki = w0*Kp;
Gcpi = Kc * tf([Kp Ki], [1 0])
[gm,pm,wg,wp] = margin (Kp*GpH); pm
%figure ('Name', 'PI Bode'); bode (Gcpi*GpH)
%figure ('Name', 'PI Nyquist'); nyquist (Gcpi*GpH, {.5, 1000})

% PD and PID controllers with various integral gains and 50 degree phase margin
disp (' ');  disp ('PD and PID controllers')
Ki = [0 0.1 1.0 5];
Ts = 4, pm = 50*pi/180; disp('pm = 50')
w1 = 8 / (Ts * tan (pm));
GpHjw1 = evalfr(GpH, j*w1);
mag = abs(GpHjw1);
theta = -pi + pm - angle (GpHjw1);
Kp = cos(theta) / mag;
t = 0:0.1:20;
figure('Name', 'PID step responses for various Ki values');
for i = 1:length(Ki)
    Kd(i) = ((sin(theta) / mag) + Ki(i)/w1) / w1;
    Gcpid(i) = tf([Kd(i) Kp Ki(i)], [1 0]);
    step (Gcpid(i)*Gp / (1 + Gcpid(i)*GpH), t)
    hold on;
end
legend (['Ki = ' num2str(Ki(1))], ['Ki = ' num2str(Ki(2))], ['Ki = ' num2str(Ki(3))], ['Ki = ' num2str(Ki(4))]);
hold off;
disp (' ');  disp ('PD controller')
Kp, disp (['Ki = ' num2str(Ki(1))]), disp (['Kd = ' num2str(Kd(1))])
disp (' ');  disp ('PID controller')
Kp, disp (['Ki = ' num2str(Ki(2))]), disp (['Kd = ' num2str(Kd(2))])
%figure ('Name', 'PID Bode'); bode (Gcpid(2)*GpH)
%figure ('Name', 'PID Nyquist'); nyquist (Gcpid(2)*GpH, {.5, 1000})

% compare all compensators
figure('Name', 'step response comparison for all controllers');
step (Gcp*Gp / (1 + Gcp*GpH), t)
hold on; step (Gcpi*Gp / (1 + Gcpi*GpH), t)
hold on; step (Gcpid(1)*Gp / (1 + Gcpid(1)*GpH), t)
hold on; step (Gcpid(2)*Gp / (1 + Gcpid(2)*GpH), t)
legend ('P', 'PI', 'PD', 'PID');
hold off

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