
%% Exported from Jupyter Notebook
% Run each section by placing your cursor in it and pressing Ctrl+Enter

%% Code Cell[11]:

clear
save_figures = 1;

%% Markdown Cell:
% Let a system have plant transfer function
% \begin{align}
% \frac{1}{(s+2)(s+6)(s+11)}.
% \end{align}
% Design a PD controller such that the closed-loop settling time is about $0.8$ seconds and the overshoot is about $15$%.

%% Markdown Cell:
% ### Determining $\psi$

%% Markdown Cell:
% We use Matlab for the design.\footnote{See \href{http://ricopic.one/control/source/pd_controller_design_example.m}{ricopic.one/control/source/pd_controller_design_example.m} for the source.
% First, we must determine what the specified transient response criteria imply for the locations of our closed-loop poles.
% Let one of these desired pole locations be called $\psi$.
% The transient response performance criteria are as follows.

%% Code Cell[12]:

Ts = .8; % sec ... spec settling time
OS = 15; % percent ... spec overshoot

%% Markdown Cell:
% The second-order approximation from \cref{ch:transientresponseperformance} tells us that the settling time specification implies a specific $\Re(\psi)$ and the overshoot a specific angle $\angle\psi$.
% The real part is found from the expressions
% \begin{gather}
%     T_s = \frac{4}{\zeta\omega_n} \quad \text{and} \quad
%     \Re(\psi) = -\zeta\omega_n \Rightarrow \\
%     \Re(\psi) = -\frac{4}{T_s}.
% \end{gather}
% The angle is found via the equations
% \begin{gather}
%     \zeta = \frac{-\ln(\%OS/100)}{\sqrt{\pi^2+\ln^2(\%OS/100)}}, 
%     \\
%     \tan(\angle\psi) = \frac{\sqrt{1-\zeta^2}}{\zeta}, 
%     \quad \text{and} \quad
%     \tan(\angle\psi) = -\Im(\psi)/\Re(\psi).
% \end{gather}
% A remarkably simple expression results:
% \begin{subequations}
% \begin{align}
%     \Im(\psi) &= -\Re(\psi) \frac{\sqrt{1-\zeta^2}}{\zeta} \\
%     \Im(\psi) &= -\Re(\psi) \frac{\pi}{\ln(100/\%OS)}.
% \end{align}
% \end{subequations}
% So, in the final analysis, the desired pole location $\psi$ (assuming the second-order approximation is valid) is given by the expression
% \begin{align}
%     \psi = -\frac{4}{T_s}
%     \left(
%         1-j \frac{\pi}{\ln(100/\%OS)}
%     \right).
% \end{align}
% This formula holds beyond the scope of this problem.
% We define it as an anonymous function.

%% Code Cell[13]:

psi_fun = @(Ts,pOS) -4/Ts*(1-1j*pi/log(100/pOS));
psi = psi_fun(Ts,OS);
disp(sprintf('psi = %0.3g + j %0.3g',real(psi),imag(psi)))

%% Markdown Cell:
% ### P control

%% Markdown Cell:
% We design a proportional controller that gets us as close as possible to $\psi$.
% The root locus is shown in \autoref{fig:pd_controller_design_example_01}.

%% Code Cell[14]:

G = zpk([],[-2,-6,-11],1);
figure
rlocus(G)

%% Code Cell[15]:

h = figure;
[r,k] = rlocus(G);
for j = 1:numel(r)/length(r)
    plot(...
        real(r(j,:)),imag(r(j,:)),...
        'color','blue',...
        'linewidth',1 ...
    );hold on
    l(1) = plot(real(r(j,1)),imag(r(j,1)),'x',...
        'MarkerSize',7,...
        'LineWidth',.75, ...
        'Color','red' ...
    );
    l(2) = plot(real(r(j,end)),imag(r(j,end)),'o',...
        'MarkerSize',7,...
        'LineWidth',.75, ...
        'Color',[0,.8,0] ...
    );
end
xlim([-20,5])
ylim([-20,20])
hgsave(h,'figures/temp');

%% Code Cell[16]:

if save_figures
  fn = 'pd_controller_design_example_01.tex';
  h = hgload('figures/temp');
  cleanfigure;
  matlab2tikz(['figures/',fn],...
    'figurehandle',h,...
    'width','.9\linewidth',...
    'height','.5\linewidth',...
    'extraAxisOptions',...
    {...
        'axis lines=left,',...
        'axis x line*=middle,',...
        'axis y line*=middle,',...
        'black!40!white,',...
        'title style={black},',...
    },...
    'showInfo', false...
  );
end

%% Markdown Cell:
% Although we cannot get close to $\psi$ on the root locus, we can at least meet our $\%OS$ specification by choosing a gain of about
% \begin{align}
%     K_1 = 240.
% \end{align}
% Let's construct the compensator and corresponding closed-loop transfer function $G_{P}$ for gain control.

%% Code Cell[18]:

K1 = 240;
G_P = feedback(K1*G,1);

%% Markdown Cell:
% ### Derivative compensation
% 
% Now, we use cascade derivative compensation with compensator
% \begin{align}
%     K_2 (s-z_c).
% \end{align}
% For now, we set $K_2 = 1$.
% From \autoref{eq:pd_zc}, we compute the compensator zero
% \begin{align*}
%     z_c = \Re(\psi) - \abs{\Im(\psi)}/\tan\theta_c \quad
%     \text{and} \quad
%     \theta_c = \pi- \angle G(\psi).
% \end{align*}

%% Code Cell[23]:

theta_c = pi - angle(evalfr(G,psi));
z_c = real(psi) - abs(imag(psi))/tan(theta_c);
disp(sprintf('theta_c = %0.3g deg',rad2deg(theta_c)))
disp(sprintf('z_c = %0.3g',z_c))

%% Markdown Cell:
% Let's construct the compensator sans tuned gain $K_2$ and tune it up using another root locus.

%% Code Cell[24]:

C_sans = zpk(z_c,[],1);
figure
rlocus(K1*C_sans*G)

%% Code Cell[25]:

h = figure;
[r,k] = rlocus(K1*C_sans*G);
for j = 1:numel(r)/length(r)
    plot(...
        real(r(j,:)),imag(r(j,:)),...
        'color','blue',...
        'linewidth',1 ...
    );hold on
    l(1) = plot(real(r(j,1)),imag(r(j,1)),'x',...
        'MarkerSize',7,...
        'LineWidth',.75, ...
        'Color','red' ...
    );
    l(2) = plot(real(r(j,end)),imag(r(j,end)),'o',...
        'MarkerSize',7,...
        'LineWidth',.75, ...
        'Color',[0,.8,0] ...
    );
end
xlim([-20,5])
ylim([-20,20])
hgsave(h,'figures/temp');

%% Code Cell[26]:

if save_figures
  fn = 'pd_controller_design_example_02.tex';
  h = hgload('figures/temp');
  cleanfigure;
  matlab2tikz(['figures/',fn],...
    'figurehandle',h,...
    'width','.9\linewidth',...
    'height','.5\linewidth',...
    'extraAxisOptions',...
    {...
        'axis lines=left,',...
        'axis x line*=middle,',...
        'axis y line*=middle,',...
        'black!40!white,',...
        'title style={black},',...
    },...
    'showInfo', false...
  );
end

%% Markdown Cell:
% The resulting root locus of \autoref{fig:pd_controller_design_example_02} intersects $\psi$!
% (I mean, we knew it would, but we had our doubts.)
% The corresponding gain is, from \autoref{eq:gainfrommagcriterion} (or we could use the data cursor),
% \begin{align}
%     K_2 = \frac{1}{\abs{(\psi-z_c)G(\psi)}}.
% \end{align}
% Let's compute it, the controller $C_{PD}$, and the closed-loop transfer function $G_{PD}$.

%% Code Cell[29]:

K2 = 1/abs(evalfr(K1*C_sans*G,psi));
C = K1*K2*C_sans;
G_PD = feedback(C*G,1);

%% Markdown Cell:
% ### Simulate
% 
% Our placement of the $\psi$ depended on the second-order approximation's accuracy, which in this case is questionable, due to the proximity of a third closed-loop pole.
% In any case, we simulate the step response to test the efficacy of the PD controller design and to compare it with the P controller.

%% Code Cell[38]:

t_a = linspace(0,2.5,200); % s ... sim time
y_P = step(G_P,t_a); % P controlled step response
y_PD = step(G_PD,t_a); % PD controlled step response

%% Code Cell[39]:

figure
plot(t_a,y_P);
hold on;
plot(t_a,y_PD);
xlabel('time (s)');
ylabel('step response');
grid on
legend('P control','PD control','location','southeast');

%% Code Cell[40]:

h = figure;
p1 = plot(t_a,y_P);
hold on;
p2 = plot(t_a,y_PD);
xlabel('time (s)',...
  'interpreter','latex');
ylabel('step response',...
  'interpreter','latex');
grid on
l = legend('P control','PD control','location','southeast');
set(l,'interpreter','latex');
hgsave(h,'figures/temp');

%% Code Cell[46]:

if save_figures
  fn = 'pd_controller_design_example_03.tex';
  h = hgload('figures/temp');
  cleanfigure;
  matlab2tikz(['figures/',fn],...
    'figurehandle',h,...
    'width','.9\linewidth',...
    'height','.5\linewidth',...
    'extraAxisOptions',...
    [...
      'scaled ticks=false,',... 
      'tick label style={/pgf/number format/fixed}'...
    ],...
    'showInfo', false...
  );
end

%% Markdown Cell:
% The responses, shown in \autoref{fig:plag_controller_design_03}, suggest the PD controller is at least close to meeting the transient specifications.
% It is a happy accident that the steady-state error also improved; derivative compensation does not always do this.
% Let's use `stepinfo` to compute more accurate transient response characteristics of the PD-controlled system.

%% Code Cell[45]:

si_PD = stepinfo(y_PD,t_a);
disp(sprintf('settling time: %0.3g',si_PD.SettlingTime))
disp(sprintf('percent overshoot: %0.3g',si_PD.Overshoot))

%% Markdown Cell:
% This is quite close to the specification.
% If desired, the gain $K_2$ and the zero location $z_c$ could be tuned, iteratively.
