% To plot figures: set plot_fig = 1
plot_fig = 0;
% Select lag compensator: original design (1) or tuned (2)
lag_controller = 1;
%
% Step amplitude (rad/s)
R0 = 5.0;
%
%% Compensator Design
% Plant transfer function
P = tf( K, [1 0] );
%
% Proportional gain (V.s/rad)
if lag_controller == 1
    % A) Design to get wg = 50 rad/s (lead adds gain which will 
    %    increase the  wg later).
    Kc = 10^(0/20);
else
    % (B) Gain to compensate for steady-state error present in actual system
    % Steady-state error measured
    e_ss_meas = 0.166;
    % Steady-state error is to be minimized to
    e_ss_min = 0.025;
    % Desired gain
    Kc = 10^(e_ss_meas/e_ss_min/20);
end
%
% Lag compensator parameter 
if lag_controller == 1
    a = 10^(-14.4/20); % for 30.0 rad/s, original control
else
    a = 10^(-21/20); % for 30.0 rad/s, after compensation
end
% Lead compensator parameter: T
T = 10 / (wg_des * a);
%
% Lag Compensator
LAG = tf([a*T 1],[T 1]);
% Complete Compensator: C(s) = (1+a*Ts)/(1+T*s)*Kc
C = Kc*LAG;
% Loop transfer function: Lp(s) = Kc*P(s)/s
Lp = Kc*P;
% Loop transfer function: L(s) = C(s)*P(s)
L = series(P,C);
% Closed-loop system
G = feedback(L,1);
%
%% Plots
if plot_fig == 1
    % Bode of plant
    figure(1)
    margin(P);
    set (1,'name','P(s)');
    % Bode of gain compensated plant Lp(s) = Kc*P(s)
    figure(2)
    margin(Lp);
    set (2,'name','Kc*P(s)');
    % Bode of Lead compensator
    figure(3)
    margin(LAG)
    set (3,'name','Lag');
    % Bode of loop transfer function L(s)
    figure(4);
    margin(L);
    set (4,'name','L(s)');
    % Step response
    figure(5)
    step(R0*G);
    stepinfo(R0*G)
    set (5,'name','Step Response');
end