% To plot figures: set plot_fig = 1
plot_fig = 0;
%
%% Specifications
% Step amplitude (rad/s)
R0 = 5.0; % low-gear = 10.0
% Desired steady-state error (rad)
e_ss_des = 0;
% Desired crossover frequency (rad/s)
wg_des = 75.0;
% Desired phase margin (deg)
PM_des = 75.0;
%
%% Compensator Design
% Plant transfer function
P = tf([K],[tau 1]);
% Integrator transfer function
I = tf([1],[1 0]);
% Plant with Integrator transfer function
Pi = series(P,I);
% Proportional gain (V.s/rad): design to get wg = 50 rad/s (lead adds gain
% which will increase the wg later)
Kc = 10^(34.5/20); % low-gear = 10^(23.77/20)
% Phase margin of L(s) = Kc*P(s)/s (deg)
PM_meas = 38.2; % low-gear = 21.5
% Maximum phase of lead (rad)
phi_m = (PM_des - PM_meas + 5) * pi / 180;
% Gain needed to get desired phase margin
a = (1+sin(phi_m)) / (1-sin(phi_m));
% High-frequency gain of lead compensator (dB)
gain_hf = 20*log10(a);
% Gain in Kc*P(s)/s where wm should be at is -a/2, i.e the
% negative of half the high-frequency gain (dB)
g_m = -a/2;
g_m_db = -10*log10(a);
% Frequency where max lead phase should occur (rad/s)
w_m = 80.9; %91.4;
% Lead compensator parameter: T
T = 1 / (w_m * sqrt(a));
% Lead Compensator
LEAD = tf([a*T 1],[T 1]);
% Complete Compensator: C(s) = (1+a*Ts)/(1+T*s)*Kc/s
C = Kc*series(LEAD,I);
% Loop transfer function: Lp(s) = Kc*P(s)/s
Lp = Kc*series(P,I);
% 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 Pi(s)
    figure(1)
    margin(Pi);
    set (1,'name','Pi(s)');
    % Bode of Kc*Pi(s)
    figure(2)
    margin(Lp);
    set (2,'name','Kc*Pi(s)');
    % Bode of Lead compensator
    figure(3)
    margin(LEAD)
    set (3,'name','Lead');
    % 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