%% Lab 12 - Robust Control Design
% In this seminar, you will design a robust controller for an unceratin servo system by mixed sensitivity loop shaping with a general controller structure and with a fixed controller structure (robust PID).
% The servo system is modeled by the following state-space model
%
% \begin{array}{rl}
%  L\frac{\mathrm{d}i(t)}{\mathrm{dt}} &= -R\,i(t) - k\,\omega(t) + u(t),\\
% J\frac{\mathrm{d}\omega(t)}{\mathrm{dt}} &= k\,i(t) - b\,\omega(t),
% \end{array}
% with parameters:

L = 30e-3;  % Motor winding inductance [H]
R = 3.4;    % Motor winding resistance [Ohm]
k = ureal('k', 0.05, 'percentage', 15);   % Motor constant (torque and back emf) [N.m/A]
J = ureal('J', 0.01, 'range', [0.005 0.02]);   % Moment of inertia [kg.m^2]
b = 0.1;    % Coefficient of viscous friction [N.m.s]

%
% Notice, that parameters k and J are uncertain. The uncertain state-space model of the servo system is created by the following commands:

A = [-R/L   -k/L;  % Current [A]
     k/J    -b/J;];  % Angular velocity [rad/s]
B = [1/L;0];  % Voltage [V]
C_pid = [0 1];     % Angular velocity [rad/s]

G = ss(A, B, C_pid, 0, ...
        'StateName',{'Current' 'Ang. Velocity'}, ...
        'StateUnit',{'A' 'rad/s'}, ...
        'InputName','Voltage', ...
        'InputUnit','V', ...
        'OutputName',{'Ang. Vel'}, ...
        'InputUnit','N.m');

% Extract the nominal model ...
G0 = G.NominalValue;
% ... and randomly sample a model for simulation purposes.
G_rand = usample(G, 1);

% Simulate step responses of several randomly sampled models.
figure(1)
step(G, G0, 'r')
legend('Sampled uncertain', 'Nominal', 'Location', 'SouthEast')

%
% Before you start designing a robust controller, you have to convert the model with parametric uncertainties to a model with dynamic uncertainties. Since you have already practiced this procedure during one of the previous seminars, we provide you with the code doing this transformation and let you focus on designing the controllers.

% Randomly sample 20 models of the servo system.
GArray = usample(G,20);

% Get frequency responses of the sampled models ...
w = logspace(-1,4,50); % frequency grid  
GArray_frd = frd(GArray, w);  
% ... and of the nominal model 
G0_frd = frd(G0, w);

% Use ucover() to find the multiplicative input uncertainty shaping filter
% $W_3$. Function ucover() takes as the third argument order of the shaping
% filter. Choose the order as small as possible because the larger the
% order of W_3 the larger the order of the resulting robust regulator.
[P,Info] = ucover(GArray_frd, G0_frd, 1);
W_3 = Info.W1;

% Check that W_3 truly is an envelope of the relative errors of the
% sampled frequency responses from the nominal frequency response.
relErr = (G0_frd - GArray_frd)/G0;
figure(3)
bodemag(relErr,'b--', W_3,'r',{0.1,10000})

%
%% Task Specification
% Design a robust controller for the uncertain model of the servo system by mixed-sensitivity loop shaping (use mixsyn() and hinfstruct()). The controller must suppress an output disturbance of frequencies up to 2 rad/s and with amplitude up to .1. In addition, the maximum steady-state error of the response to a unit step in reference must be smaller than 0.01. Verify by simulations that your controller satisfies these design requirements.
% Note: We use the notation from the documentation of function mixysn(), that means W_1 determines the shape of sensitivity transfer function and W_3 determines the shape of the complementary senstivity transfer function. Shaping filter W_2 penalizes the control inputs.
%
% Proceed as follows:
% Shaping filters
% You already have the shaping filter W_3 because W_3 corresponds to the shaping filter of the multiplicative input uncertainty.
% Choose the performance shaping filter W_1 so that the design requirements are met. You can do it either by hand or preferably you can use makeweight() function.
% General Mixed-sensitivity (Section 9.3.5 in Skogestad's textbook)
% Desing a robust controller by mixsyn() function. At first, set the shaping filter W_2 to zero. Recall that this shaping filter effectively expresses the cost of the control.
% Simulate a step response of the closed-loop system for a randomly sampled uncertain servo system. If you do not find the response satisfactory, modify the shaping filters W_1, W_2 and W_3 and repeat the previous step.
% Robust PID
% Augment the nominal model by shaping filters W_1, W_2 and W_3 (use augw() function).
% Define a tunable PID controller (C0_pid = tunablePID('C', 'pid')).
% Design a robust PID by calling function hinfstruct() for the augmented model the tunable PID.
% Simulate a step response of the closed-loop system for a randomly sampled uncertain servo system. If you do not find the response satisfactory, modify the shaping filters W_1, W_2 and W_3 and repeat the design procedure.