%% Closed-Loop Position Control of a Tank System: the Case of a Proportional Control Law
%
%
% Table of Contents
% System Description
%     Dynamic Modeling - Different Scenarios: No Disturbance vs Step-like Disturbance
% Closed-Loop Control Strategy using a Proportional Controller : Performance Analysis
%         First Scenario
%         Second Scenario
%     The Dynamic Model  & the Control Strategy in Matlab
%     Performance Analysis
%         Initial Conditions
%         Setting the Proportional Gain
%         System Parameters
%         Set Point
%         Simulation Time Interval
%     Solving an Ordinary Differential Equation in MATLAB - First Scenario - No Disturbance
%         Simulation of the Controlled System - Solving the ODE - First Scenario: No Disturbance
%         The Simulation Results: the Behavior of the Controlled System - First Scenario: No Disturbance
%     Solving an Ordinary Differential Equation in MATLAB - Second Scenario - Step-like Disturbance
%         Setting the Disturbance
%         Simulation of the Controlled System - Solving the ODE - Second Scenario: Step-like Disturbance
%         The Simulation Results: the Behavior of the Controlled System - Second Scenario: Step-like Disturbance
% The ODE as MATLAB Function
%
%% System Description
%
% Consider the hydraulic system
%
% where
% control input: the input flow-rate q_e
% controlled output: the level of liquid in the tank h
% reference output: the constant desired level in the tank w = h^{o}
% out-flow rate:: q_u = k \cdot h
% disturbance: d
% A real system that can be described by the model we are analyzing is, for example, a paint mixing tank:
%
%
% Paint mixing tanks (automotive industry).
%
%% Dynamic Modeling - Different Scenarios: No Disturbance vs Step-like Disturbance
%
% First Scenario - Usual assumptions:
% infinite-height of the tank (so we disregard the static nonlinearity due to the level saturation);
% not compressible liquid: in this way we may apply a simple volume conservation equation instead of the mass balance equation;
% no disturbance at all: \forall t \; d(t)=0 \;
%
% \text{volume time variation } \;=\;  \text{input flow-rate }\;-\; \text{output flow-rate}
%  \quad \Longrightarrow \quad
% \left\{ \begin{array}{l}
% A\,\dot{h}(t) = q_e(t)- q_u(t) -d(t) \\
% d(t) = 0
% \end{array} \right. \quad \Longrightarrow \quad \dot{h}(t) = \frac{1}{A}\,q_e(t) - \frac{k}{A}\,h(t)
%
%
% Second Scenario - Alternative assumptions:
% suppose a step-like disturbance d(t) = D \cdot \mathbf{1(t)}, where D \in \mathbb{R},\,D>0 and \mathbf{1(t)} is the Heaviside function
% \mathbf{1(t)} := \left\{ \begin{array}{rcl}
% 1 &: & t \ge 0 \\ \\
% 0 &:& t<0
% \end{array} \right.
%
%
% \text{volume time variation } \;=\;  \text{input flow-rate }\;-\; \text{output flow-rate}
%  \quad \Longrightarrow \quad
% \left\{ \begin{array}{l}
% A\,\dot{h}(t) = q_e(t)- q_u(t) -d(t) \\
% d(t) = D \cdot 1(t)
% \end{array} \right. \quad \Longrightarrow \quad \dot{h}(t) = \frac{1}{A}\,q_e(t) - \frac{k}{A}\,h(t) - \frac{1}{A}\,D\cdot 1(t)
%
%
%
%% Closed-Loop Control Strategy using a Proportional Controller : Performance Analysis
%
% We would like to control the liquid level h by applying a simple closed-loop control strategy: the control action is proportional to the error between the desired set-point (the assigned liquid level set-point) and the actual value of the controlled variable (the liquid level in the tank)
% q_e(t) = \mu\,\big(h^{o} - h(t) \big)\;, \quad \mu >0
%% First Scenario
%
%
% The system dynamics is now described by:
% A \dot{h}(t) + k\,h(t) = \mu\,\big( h^{o} - h(t) \big)
% with initial condition h(0).
%
%% Second Scenario
%
%
% The system dynamics is now described by:
% A \dot{h}(t) + k\,h(t) +d(t) = \mu\,\big( h^{o} - h(t) \big)
% with initial condition h(0).
%
%% The Dynamic Model  & the Control Strategy in Matlab
%

% --------------- feedback control using a P regulator -------------------
%
% The current script allows to simulate the feedback control of the tank
% system using a simple P controller, as described in Part-1 of the course
% material. 
%
% To perform the simulation, the script uses the MATLAB command ode
% -----------------------------------------------------------------------
clear % let's clean the Matlab workspace
close all
clc

%
%% Performance Analysis
% How does the system behave when the initial conditions change?
% Does the dynamic behavior of the system depend on the system's parameters and the controller's parameter \mu or only on the system's parameters A,\,k\,?
% Using the closed-loop P control strategy are we able to impose a desired dynamic behavior to the system?
% To answer these questions, try changing the initial conditions, or the system parameters (or all of these) and simulate the behavior of the system.
%
%
%% Initial Conditions
%
% Modify the initial liquid level in the tank h(0) and analyse the result

 

h_init = 1.388; % [m]
% initial level of fluid into the tank

% Check the simulation result: no disturbance scenario.
% Check the simulation result: step disturbance scenario.
%
%% Setting the Proportional Gain
% This is the unique controller parameter: setting the proportional gain \mu we are able to modify the controlled system behavior.
% What would happen if we choose a very small positive value for \mu? What if we increase the gain?
% Would the performance of the control system remain the same? Would it improve? Would it get worse?
% Experiment with what happens if you select a different value.
% For example, what if you use a value 10 times smaller or 10  times larger?
% Does the performance change?
% Does the steady-state error change or remain the same?

muP = 200;

% Modify the P gain value and simulate. What happens to the
% controlled system performance?
      
 

% Check the simulation result: no disturbance scenario.
% Check the simulation result: step disturbance scenario.
%
%% System Parameters
% The nominal set of parameters' values is as follows:
% \begin{array}{rclcrcl}
% A&=& 12.34\,\text{m}^2 & \quad & h&=& 0.89 \, \text{m}^2\text{/s}
% \end{array}

 

% 
A = 12.34;   % [m^2] area of the tank section
k = 0.89;    % [m^2/s]  k is the coefficient of the linear law for 
                             % calculating the outlet flow rate q_out as a function of the 
                             % liquid level h in the tank
                             %
                             % q_out = k * h
                             %

% Check the simulation result: no disturbance scenario.
% Check the simulation result: step disturbance scenario.
%
% In fact, given the differential equation
% A\, \dot{h} + k \,h = \mu \left(h^{o} - h\right)
% the associated polynomial algebraic equation is
% A\, \lambda+ \left(k +\mu\right)= 0
% and its roots are

roots_CharPolyODE = roots([A (k+muP)])

% Note: the system behavior depends either on the system's parameters ( A,\,k\,) and on the controller gain \mu.
% What happens if you increase the controller's gain \mu?
%
%% Set Point
% Assign the desired liquid level.
% Can the current control law guarantee that the desired level is reached?

 

% set point
h_0 = 8.56;    % [m] desired fluid level in the tank

% Check the simulation result: no disturbance scenario.
% Check the simulation result: step disturbance scenario.
%
%% Simulation Time Interval
% Set the duration of the simulation:

 

Tfinal = 7; % [s] simulation in the time internal [0 Tfinal]

% Check the simulation result: no disturbance scenario.
% Check the simulation result: step disturbance scenario.
%
%% Solving an Ordinary Differential Equation in MATLAB - First Scenario - No Disturbance
%
% We would solve the first-order Ordinary Differential Equation (ODE)
% A\, \dot{h} + k \,h = \mu \left(h^{o} - h\right) \;, \qquad h(0) =\bar h
% in MATLAB, numerically, using the command ode.
% For help on using the command ode, and examples, refer to the online documentation or execute the following piece of code in the MATLAB Command Window
% doc ode
%
% Briefly, the ode command allows us to solve Initial Value Problems (IVPs) of the form
% \dot{y}(t) = f\big(t,\,y(t), p \big)\,,\quad \dot{y}(t) = \frac{d\,y(t)}{dt}
% where y is a continuous function of the independent variable t (i.e., the solution of the ODE we are looking for), and p is a vector of parameters.
%
% Thus, given the first-order ODE to solve, we must perform a single-step procedure to obtain the (numerical) ODE solution using the MATLAB command ode:
%
% One-Step Procedure: Define the system of first-order ODEs with given initial conditions in MATLAB, using the command ode
% \left\{ \begin{array}{rcl}
% \dot{y}_1(t) &=& -\frac{k+\mu}{A}\,y_1(t)  + \frac{\mu}{A}\, h^{o} \\ \\
% y_1(0) &= & \bar h
% \end{array} \right.  \qquad \Longrightarrow \qquad  \begin{array}{rcl}
% y_1(t) &=& ?
% \end{array}

% Note: using the default configuration for the current live script, all
% the MATLAB code is hidden, so you cannot see this portion of code, unless
% you modify the live script general visualization properties.
%
F_ODE = ode;                              % let's initialize an ODE object
F_ODE.Parameters   = [A, k, h_0, muP, 0]; % the parameters of the differential equation

% NOTE: We also pass the amplitude D of the step-like disturbance d(t)
% [actually D=0 in this scenario] as a parameter. 
% For practicality, we prefer to manage only one ODE, which can cope 
% with both scenarios, just configuring the parameters appropriately. 

F_ODE.InitialValue = h_init;              % the initial conditions
F_ODE.InitialTime = 0;                    % the initial time instant of the solution, where 
                                          % the initial conditions are applied
F_ODE.ODEFcn = @TankPFeedbackODE;   % TankPFeedbackODE is the MATLAB function describing the ODE, 
                                    % we would like to solve. 
                                    % The code of TankPFeedbackODE is at the end
                                    % of this live script.

%
%% Simulation of the Controlled System - Solving the ODE - First Scenario: No Disturbance
%
% Given the MATLAB description of the IVP we would solve, let's determine a numeric solution by applying the MATLAB command solve.
% For help on using the command solve, and examples, refer to the online documentation or execute the following piece of code in the MATLAB Command Window
% doc solve
% Briefly, the solve command computes the solution of the considered IVP F in the time interval [t0, tf], returning the solution S at each time step chosen by the solver:
% S = solve(F, t0, tf);

% Note: using the default configuration for the current live script, all
% the MATLAB code is hidden, so you cannot see this portion of code, unless
% you modify the live script general visualization properties.
%
t0 = 0; % the initial time instant
% tf already assigned
S  = solve(F_ODE, t0, Tfinal); % solve the IPV described by F_ODE 
                           % in the time interval [t0, tf]

tank_level = S.Solution; % the result of the IPv
t_stamps   = S.Time;     % time instants corresponding to the solution samples 


% the steady-state level
h_steady = (muP/(muP+k))*h_0;

%
%
%% The Simulation Results: the Behavior of the Controlled System - First Scenario: No Disturbance
%

% Note: using the default configuration for the current live script, all
% the MATLAB code is hidden, so you cannot see this portion of code, unless
% you modify the live script general visualization properties.
%
figure('Units','normalized','Position',[0.1, 0.1, 0.95, 0.95]);

plot(t_stamps, tank_level, 'LineWidth',2); % cart position vs time
hold on; % let's add another plot: the set-point
plot(t_stamps, h_0*ones(size(t_stamps)),'LineStyle','--','LineWidth',1.5); 
plot(t_stamps, h_steady*ones(size(t_stamps)),'LineStyle','-.','LineWidth',1.5); 


xlabel('time $t$ [s]', 'FontSize', 14, 'Interpreter','latex'); % labels on the plot axes
ylabel('level $h$ [m]', 'FontSize', 14, 'Interpreter','latex'); 

grid on; % let's insert a grid in the figure
title('P Feedback Control of a Tank: No Disturbance Scenario');

legend('liquid level $h$', 'set point $h^{o}$', 'Steady state level $h_{\infty}$',...
        'Interpreter', 'latex', 'FontSize', 14, 'Location', 'best' );% adding a legend

%
% Modify the simulation time interval.
% Assign the set point.
% Modify the system parameters.
% Set the controller gain.
% Assign the initial conditions.
%
%% Solving an Ordinary Differential Equation in MATLAB - Second Scenario - Step-like Disturbance
%
%% Setting the Disturbance
% Let's assign the amplitude of the step-like disturbance;

% disturbance d amplitude
    
D_dV = 3.9; % [m^3/s] disturbance flow rate. ACTIVE DISTURBANCE SCENARIO


 

% Check the simulation result: no disturbance scenario.
% Check the simulation result: step disturbance scenario.
%
% We would solve the first-order Ordinary Differential Equation (ODE)
% A\, \dot{h} + k \,h +D\cdot 1(t)= \mu \left(h^{o} - h\right) \;, \qquad h(0) =\bar h,\;D \neq 0
% in MATLAB, numerically, using the command ode.
% For help on using the command ode, and examples, refer to the online documentation or execute the following piece of code in the MATLAB Command Window
% doc ode
%
% Briefly, the ode command allows us to solve Initial Value Problems (IVPs) of the form
% \dot{y}(t) = f\big(t,\,y(t), p \big)\,,\quad \dot{y}(t) = \frac{d\,y(t)}{dt}
% where y is a continuous function of the independent variable t (i.e., the solution of the ODE we are looking for), and p is a vector of parameters.
%
% Thus, given the first-order ODE to solve, we must perform a single-step procedure to obtain the (numerical) ODE solution using the MATLAB command ode:
%
% One-Step Procedure: Define the system of first-order ODEs with given initial conditions in MATLAB, using the command ode
% \left\{ \begin{array}{rcl}
% \dot{y}_1(t) &=& -\frac{k+\mu}{A}\,y_1(t)  + \frac{\mu}{A}\, h^{o} -\frac{1}{A}\,D\cdot1(t) \\ \\
% y_1(0) &= & \bar h
% \end{array} \right.  \qquad \Longrightarrow \qquad  \begin{array}{rcl}
% y_1(t) &=& ?
% \end{array}

% Note: using the default configuration for the current live script, all
% the MATLAB code is hidden, so you cannot see this portion of code, unless
% you modify the live script general visualization properties.
%
F_ODE_d = ode;                              % let's initialize an ODE object
F_ODE_d.Parameters   = [A, k, h_0, muP, D_dV]; % the parameters of the differential equation

% NOTE: We also pass the amplitude D of the step-like disturbance d(t)
% as a parameter. 
% For practicality, we prefer to manage only one ODE, which can cope 
% with both scenarios, just configuring the parameters appropriately. 

F_ODE_d.InitialValue = h_init;              % the initial conditions
F_ODE_d.InitialTime = 0;                    % the initial time instant of the solution, where 
                                          % the initial conditions are applied
F_ODE_d.ODEFcn = @TankPFeedbackODE;   % TankPFeedbackODE is the MATLAB function describing the ODE, 
                                    % we would like to solve. 
                                    % The code of TankPFeedbackODE is at the end
                                    % of this live script.

%%
%
%% Simulation of the Controlled System - Solving the ODE - Second Scenario: Step-like Disturbance
% Given the MATLAB description of the IVP we would solve, let's determine a numeric solution by applying the MATLAB command solve.
% For help on using the command solve, and examples, refer to the online documentation or execute the following piece of code in the MATLAB Command Window
% doc solve
% Briefly, the solve command computes the solution of the considered IVP F in the time interval [t0, tf], returning the solution S at each time step chosen by the solver:
% S = solve(F, t0, tf);

% Note: using the default configuration for the current live script, all
% the MATLAB code is hidden, so you cannot see this portion of code, unless
% you modify the live script general visualization properties.
%
t0 = 0; % the initial time instant
% tf already assigned
S_d  = solve(F_ODE_d, t0, Tfinal); % solve the IPV described by F_ODE 
                           % in the time interval [t0, tf]

tank_level = S_d.Solution; % the result of the IPv
t_stamps   = S_d.Time;     % time instants corresponding to the solution samples 


% the steady-state level
h_steady = (muP/(muP+k))*h_0;

%
%
%% The Simulation Results: the Behavior of the Controlled System - Second Scenario: Step-like Disturbance
%

% Note: using the default configuration for the current live script, all
% the MATLAB code is hidden, so you cannot see this portion of code, unless
% you modify the live script general visualization properties.
%
figure('Units','normalized','Position',[0.1, 0.1, 0.95, 0.95]);

plot(t_stamps, tank_level, 'LineWidth',2); % cart position vs time
hold on; % let's add another plot: the set-point
plot(t_stamps, h_0*ones(size(t_stamps)),'LineStyle','--','LineWidth',1.5); 
plot(t_stamps, h_steady*ones(size(t_stamps)),'LineStyle','-.','LineWidth',1.5); 
plot(t_stamps, D_dV*ones(size(t_stamps)),'LineStyle',':','LineWidth',1.5);

xlabel('time $t$ [s]', 'FontSize', 14, 'Interpreter','latex'); % labels on the plot axes
ylabel('level $h$ [m]', 'FontSize', 14, 'Interpreter','latex'); 

grid on; % let's insert a grid in the figure
title('P Feedback Control of a Tank: Step-like Disturbance Scenario');

legend('liquid level $h$', 'set point $h^{o}$', ...
        'Steady state level $h_{\infty}$', 'disturb $d(t)$',...
        'Interpreter', 'latex', 'FontSize', 14, 'Location', 'best' );% adding a legend

%
% Modify the simulation time interval.
% Assign the set point.
% Modify the system parameters.
% Set the controller gain.
% Assign the initial conditions.
%
%% The ODE as MATLAB Function
%
% Let's define as a MATLAB function the first-order ODE corresponding to
% \left\{ \begin{array}{rcl}
% \dot{y}_1(t) &=& -\frac{k+\mu}{A}\,y_1(t)  + \frac{\mu}{A}\, h^{o} -\frac{1}{A}\,D \cdot 1(t) \\ \\
% y_1(0) &= & \bar h
% \end{array} \right.
% The MATLAB function must provide the first derivative \dot{y}_1(t), given the values of the function y_1 at the current time instant t and the parameters' values:
% function dydt = TankPFeedbackODE(t, y, p)
%     % retrieving the parameters
%     A_val   = p(1);
%     k_val   = p(2);
%     h0_val  = p(3);
%     muP_val = p(4);
%     D_val   = p(5);
%
%     kMuA = (k_val+muP_val)/A_val;
%     D_A  = D_val/A_val;
%     U    = (muP_val/A_val)*h0_val;
%
%     dydt = -kMuA*y(1)-D_A+U;
%
% end % function
%

% Note: using the default configuration for the current live script, all
% the MATLAB code is hidden, so you cannot see this portion of code, unless
% you modify the live script general visualization properties.
%
function dydt = TankPFeedbackODE(t, y, p)
    % retrieving the parameters
    A_val   = p(1);
    k_val   = p(2);
    h0_val  = p(3);
    muP_val = p(4);
    D_val   = p(5);

    kMuA = (k_val+muP_val)/A_val;
    D_A  = D_val/A_val;
    U    = (muP_val/A_val)*h0_val;

    dydt = -kMuA*y(1)-D_A+U;

end % function

%
%
%
%
%
%