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

%% Code Cell[1]:

clear
save_figures = 1;

%% Markdown Cell:
% Let a system have plant transfer function
% \begin{align}
% \frac{s+40}{s^2+10 s+200}.
% \end{align}
% Design a PID controller such that the closed-loop rise time is about $0.1$ seconds, the overshoot is less than $5$%, and the steady-state error is zero for a step command.

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

%% Markdown Cell:
% We use Matlab for the design.\\footnote{See \href{http://ricopic.one/control/source/pid_controller_design_example_01.m}{ricopic.one/control/source/pid\_controller\_design\_example\_01.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[2]:

Tr = .05; % sec ... spec rise time
OS = 5; % percent ... spec overshoot max

%% Markdown Cell:
% The second-order approximation from \cref{ch:transientresponseperformance} tells us, via \cref{fig:ch03risetimedampingratio}, that the rise time specification implies a specific ratio between $\omega_n$ and the implicit function $f(\zeta)$ defined in \cref{fig:ch03risetimedampingratio}---or, equivalently, between $|\psi|$ and $f(\angle\psi)$:
% \begin{subequations}
% \begin{align} \label{eq:pid_ex_01_tr}
% T_r |\psi| &= f(\angle\psi) \Rightarrow \\
% T_r &= \frac{f(\angle\psi)}{|\psi|} \\
% &= 0.3. \tag{spec}
% \end{align}
% \end{subequations}
% The minimum angle is determined from the overshoot specification via the relations
% \begin{align}
%     \angle\psi &= \pi - \arccos{\zeta} \quad\text{and} \\
%     \zeta &= \frac{-\ln(\%OS/100)}{\sqrt{\pi^2+\ln^2(\%OS/100)}}.
% \end{align}
% 

%% Code Cell[3]:

zeta = -log(OS/100)/sqrt(pi^2+log(OS/100)^2)
psi_angle_min = pi - acos(zeta)

%% Markdown Cell:
% With $\zeta$ in-hand, we use \cref{fig:ch03risetimedampingratio} to determine $f(\angle\psi)$ and apply \cref{eq:pid_ex_01_tr} to determine $|\psi|=\omega_n$:

%% Code Cell[4]:

psi_mag = 2.15/Tr

%% Markdown Cell:
% So the target magnitude $|\psi|$ and minimum angle $\angle\psi$ are determined. Let's convert this to rectangular coordinates:

%% Code Cell[5]:

psi_real = psi_mag*cos(psi_angle_min);
psi_imag = psi_mag*sin(psi_angle_min);
psi = psi_real+i*psi_imag

%% Markdown Cell:
% So this is our design target for the dominant closed-loop poles. As usual, it depends on the second-order approximation, so we will need to simulate to determine the actual performance.

%% 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:pid_controller_design_example_01_01}.

%% Code Cell[6]:

G = tf([1,40],[1,10,200]);
figure
rlocus(G)

%% Code Cell[35]:

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([-100,5])
% ylim([-20,20])
hgsave(h,'figures/temp');

%% Code Cell[36]:

if save_figures
  fn = 'pid_controller_design_example_01_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 quite to $\psi$ on the root locus, we can at least try to meet our $\%OS$ specification by choosing a conservative gain of about
% \begin{align}
%     K_1 = 70.
% \end{align}
% Let's construct the compensator and corresponding closed-loop transfer function $G_{P}$ for gain control.

%% Code Cell[9]:

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

%% Markdown Cell:
% ### Derivative compensation
% 
% Now, we try 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 angle contribution
% \begin{align*}
%     \theta_c = \pi- \angle G(\psi).
% \end{align*}

%% Code Cell[10]:

theta_c = pi - angle(evalfr(G,psi));
disp(sprintf('theta_c = %0.3g deg',rad2deg(theta_c)))

%% Markdown Cell:
% We try using the zero compensator:
% \begin{align}
%     K_2 (s-z_c).
% \end{align}
% where
% \begin{align}
% z_c = \Re(\psi) - \abs{\Im(\psi)}/\tan(\theta_c)
% \end{align}

%% Code Cell[11]:

z_c = real(psi) - abs(imag(psi))/tan(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[12]:

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

%% Code Cell[37]:

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[38]:

if save_figures
  fn = 'pid_controller_design_example_01_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:
% By construction, the resulting root locus of \autoref{fig:pid_controller_design_example_01_02} intersects $\psi$.
% 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[15]:

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[16]:

t_a = linspace(0,.7,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[17]:

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[18]:

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[19]:

if save_figures
  fn = 'pid_controller_design_example_01_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:pid_controller_design_01_03}, suggest the PD controller is probably not meeting the transient performance specifications.
% Let's use `stepinfo` to compute more accurate transient response characteristics of the PD-controlled system.

%% Code Cell[20]:

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

%% Markdown Cell:
% It's too fast and overshoots too much. Our second-order approximation that led to this design is not very accurate. Before we start tuning this design, let's fix the steady-state error by including an integral compensator. Perhaps this compensator's zero can "help" us with our fix.

%% Markdown Cell:
% ### Integral compensation
% 
% The integral compensator has its usual form
% \begin{align}
% K_3 \frac{s - z_I}{s}.
% \end{align}
% We're less concerned than usual about affecting our transient response with this compensator because we need some help doing so in any case. Let's start with $z_I = -5$.

%% Code Cell[21]:

z_I = -5;
C_I_sans = zpk(z_I,0,1);

%% Markdown Cell:
% Now, a root locus wouldn't be particularly helpful here, since our second-order approximation is poor and getting worse by the minute. Instead, we proceed directly to simulation.

%% Code Cell[22]:

G_PID1 = feedback(C_I_sans*C*G,1);
y_PID1 = step(G_PID1,t_a); % PID controlled step response

%% Code Cell[23]:

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

%% Code Cell[24]:

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

%% Code Cell[25]:

if save_figures
  fn = 'pid_controller_design_example_01_04.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:pid_controller_design_01_04}, show that the steady-state error has improved with integral compensation, and so has the transient response, but not enough.

%% Code Cell[26]:

si_PID = stepinfo(y_PID1,t_a);
disp(sprintf('rise time: %0.3g',si_PID.RiseTime))
disp(sprintf('percent overshoot: %0.3g',si_PID.Overshoot))

%% Markdown Cell:
% It's still too fast and overshoots too much. At this point we can directly tweak our compensator zeros and the overall gain to try to meet our specifications.

%% Code Cell[27]:

K3 = 4;
z_D = -25;
z_I = -8;
C_D_sans = zpk(z_D,[],1);
C_I_sans = zpk(z_I,0,1);
G_PID2 = feedback(K1*K2*K3*C_I_sans*C_D_sans*G,1);
y_PID2 = step(G_PID2,t_a); % PID controlled step response

%% Code Cell[28]:

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

%% Code Cell[29]:

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

%% Code Cell[30]:

if save_figures
  fn = 'pid_controller_design_example_01_05.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

%% Code Cell[31]:

si_PID = stepinfo(y_PID2,t_a);
disp(sprintf('rise time: %0.3g',si_PID.RiseTime))
disp(sprintf('percent overshoot: %0.3g',si_PID.Overshoot))

%% Markdown Cell:
% It turns out to be difficult to meet both specifications, even with the massively tweaked controller design. Whenever one attempts to increase the rise time, the overshoot also increases.
