%% Closed-Loop Position Control of a Cart-Spring-Damper System: the Case of a Proportional and Derivative Control Law
%
%
% Table of Contents
% System Description
%     Dynamic Modeling
% Closed-Loop Control Strategy using a Proportional and Derivative Controller : Performance Analysis
%     The Dynamic Model  & the Control Strategy in Matlab
%     Performance Analysis
%         Initial Conditions
%         The Controller: Setting the Proportional and the Derivative Gains
%         System Parameters
%         Set Point
%         Simulation Time Interval
%     Solving an Ordinary Differential Equation in MATLAB
%         Simulation of the Controlled System - Solving the ODE
%         The Simulation Results: the Behavior of the Controlled System
% The ODE as MATLAB Function
%
%% System Description
%
% Consider the mechanical system
%
% control input: the external force F
% controlled output: the position of the cart x
% reference output: the desired position w = x^{o}
% elastic spring force: F_e = k x
% damper viscous friction force: F_a = h \dot{x}
% A real system that can be described by the mechanical model we are analyzing is, for example, the one depicted in the figure below:
%
% ALn501-502 "Minuetto" MD21, coupling system (Source).
%
%% Dynamic Modeling
% The effects of mass, spring, and damper are not negligible, and dynamic behaviors such as oscillations may occur.  Models capturing the dynamic modes of behavior are clearly necessary.
%
% \text{sum of all external forces } \;=\; M \ddot{x} \quad \Longrightarrow \quad F - F_e -F_a = M \ddot{x} \quad \Longrightarrow \quad M \ddot{x} +h\,\dot{x} +k\,x = F
%
%
%
%
%% Closed-Loop Control Strategy using a Proportional and Derivative Controller : Performance Analysis
%
% We would like to  the cart position x by applying a simple closed-loop control strategy: the control action is the sum of two contributions: the first is proportional to the error between the desired set-point (the assigned position set-point) and the actual value of the controlled variable (the carriage position), while the second is proportional to the derivative of the error itself.
% F = \alpha\,\big(x^{o} - x \big) + \beta\,\frac{d\,}{d\,t}\big(x^{o} - x \big) \;, \quad \alpha, \, \beta >0
%
%
%
% The system dynamics is now described by:
% M \ddot{x} + h \dot{x} + k\, x = \alpha\,\left( x^{o} - x \right)-\beta \, \dot{x}
% with initial conditions x(0) and \dot{x}(0).
%
% How does the system behave when the initial conditions change?
% Does the dynamic behavior of the system depend either on the system's parameters and on the controller's parameters \alpha,\,\beta, or only on the system's parameters M,\,h,\,k\,?
% Using the closed-loop PD control strategy are we able to impose a desired dynamic behavior to the system?
%
% Let's analyse the performance of the proposed control strategy, implementing the model and simulating its behavior in different scenarios.
%
%% The Dynamic Model  & the Control Strategy in Matlab
% The nominal set of parameters' values is as follows:
% \begin{array}{rclcrcl}
% M&=& 123.52 \,\text{kg} & \quad & h&=& 9.12 \, \text{Ns/m} \\
% \bar{k} &=& 10.09 \, \text{N/m} &\quad & k&=& 10.09 \, \text{N/m}
% \end{array}
%

% --------------- feedback control using a PD regulator -------------------
%
% The current script allows to simulate the feedback control of the cart
% system using a simple PD controller, 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 parameters \alpha,\, \beta or only on the system's parameters M,\,h,\,k\,?
% Using the closed-loop PD 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 spring constant, or the system parameters (or all of these) and simulate the behavior of the system.
%
%
%% Initial Conditions
%
% Modify the initial position x(0) and the initial speed \dot{x}(0) of the cart and analyse the result

 

init_pos   = -1.75; % initial position [m]
init_speed = -4.5; % initial speed [m/s]

% Check the simulation result.
%
%% The Controller: Setting the Proportional and the Derivative Gains
% Note: by setting the proportional gain \mathbf{\alpha} and the derivative gain \mathbf{\beta} we are able to modify the controlled system behavior.
% What would happen if we choose a very small positive value for \alpha? What if we increase the gain \alpha?
% Would the performance of the control system remain the same? Would it improve? Would it get worse?
% Modify the value of the derivative gain \beta and of the proportional gain \alpha. What happens?
% Do you obtain better performance by increasing or decreasing the gain \alpha (while keeping constant the gain \beta)?
% Conversely, what happens by increasing or decreasing the gain \beta (while keeping constant the gain \alpha)?

alphaP = 332; % <-- the P gain
betaD = 89.5;   % <-- the D gain
      
 

% Check the simulation result.
%
%% System Parameters
% The nominal set of parameters' values is as follows:
% \begin{array}{rclcrcl}
% M&=& 123.52 \,\text{kg} & \quad & h&=& 9.12 \, \text{Ns/m} \\
% \bar{k} &=& 10.09 \, \text{N/m} &\quad & k&=& 10.09 \, \text{N/m}
% \end{array}
% Note: the control law parameters \alpha,\,\beta are influencing the controlled system behavior


 

M = 123.52;  % kg [cart mass]
h = 9.12;   % Ns/m [damping constant] % 9.12;
k = 10.09;   % N/m [spring elastic coeff.] <-- change the value; use for instance k = 10.89    
k_bar = 10.09; % N/m <-- nominal value  of the spring elastic constant
% Note: we do not use this parameter in the controller <--> compare with the open-loop controller
% using this set of params --> roots([M h k]) complex conj.
% 

% Check the simulation result.
%
% In fact, given the differential equation
% M\, \ddot{x} + h \,\dot{x} + k \,x = \alpha \left(x^{o} - x\right)-\beta\,\dot{x}
% the associated polynomial algebraic equation is
% M\, \lambda^2+\left(h+\beta \right)\,\lambda + \left(k +\alpha\right)= 0
% and its roots are

roots_CharPolyODE = roots([M (h+betaD) (k+alphaP)])

% Note: the system behavior depends on the system's parameters ( M,\,h,\,k\,) and the controller gains \alpha,\, \beta.
% Change the damping constant, using for instance the value \hat{h} = 90.12 \; \text{[Ns/m]}\,and verify that the system behavior is very different if both the controller gains \alpha,\,\beta assume a very small positive value.
% If the controller gain \alpha is increased, but keeping small the value of the gain \beta, the behavior of the system becomes more oscillatory as the \alpha gain becomes higher.
% Select a medium scale value for the gain \alpha and then explore the effect of increasing the gain \beta starting from the minimum available value. What happens? How does change the system behavior?
%
%% Set Point
% Assign the desired carriage position.
% Can the current control law guarantee that the desired position is reached?

 

x0 = 7.8;    % m [desired cart position -- setpoint]

% Check the simulation result.
%
%% Simulation Time Interval
% Set the duration of the simulation:

 

finalT = 110; % final time instant for the simulation
% The simulation makes it possible to determine the behavior
% of the cart in the time interval [0, finalT].

% Check the simulation result.
%
%% Solving an Ordinary Differential Equation in MATLAB
%
% We would solve the second-order Ordinary Differential Equation (ODE)
% M\, \ddot{x} + h \,\dot{x} + k \,x = \alpha \left(x^{o} - x\right)-\beta\,\dot{x} \;, \qquad x(0) =\bar x\,,\quad \dot{x}(0) = \bar v
% 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 (vector) function (the solution of the ODE we are looking for) and p is a vector of parameters.
%
% Thus, given the second-order ODE to solve, we must perform a two-step procedure to obtain the (numerical) ODE solution using the MATLAB command ode:
%
% First Step: Let's transform the second-order ODE in a system of two first-order ODEs. Using the substitutions
% y_1(t) := x(t) \,,\qquad y_2(t) := \frac{d\,x(t)}{dt} = \dot{x}(t)
% we obtain the following system of two first-order ODEs
% \left\{\begin{array}{l}
% M\, \ddot{x} + h \,\dot{x} + k \,x = \alpha \big[ x^{o} - x\big]-\beta\,\dot{x} \\
% x(0) =\bar x\,,\quad \dot{x}(0) = \bar v \\ \\
% y_1(t) := x(t)\\
% y_2(t) := \frac{d\,x(t)}{dt} = \dot{x}(t)
% \end{array} \quad \Longrightarrow \quad
% \left\{ \begin{array}{rcl}
% \dot{y}_1(t) &=& y_2(t) \\
% \dot{y}_2(t) &=&-\frac{k+\alpha}{M}\,y_1(t) -\frac{h+\beta}{M}\,y_2(t) + \frac{\alpha}{M}\, x^{o} \\ \\
% y_1(0) &= & \bar x \\
% y_2(0) &=& \bar v
% \end{array} \right.
%
% Second Step: 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) &=& y_2(t) \\
% \dot{y}_2(t) &=&-\frac{k+\alpha}{M}\,y_1(t) -\frac{h+\beta}{M}\,y_2(t) + \frac{\alpha}{M}\, x^{o} \\ \\
% y_1(0) &= & \bar x \\
% y_2(0) &=& \bar v
% \end{array} \right.  \qquad \Longrightarrow \qquad
% \left\{ \begin{array}{rcl}
% y_1(t) &=& ?\\
% y_2(t) &=& ?
% \end{array} \right.

% 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   = [M, h, k, x0, ...
                             alphaP, betaD];  % the parameters of the differential equation
F_ODE.InitialValue = [init_pos, init_speed];  % the initial conditions
F_ODE.InitialTime = 0;                        % the initial time instant of the solution, where 
                                              % the initial conditions are applied
F_ODE.ODEFcn = @CartPDFeedbackODE;  % CartPDFeedbackODE is the MATLAB function describing the ODE, 
                                    % we would like to solve. 
                                    % The code of CartPDFeedbackODE is at the end
                                    % of this live script.

%
%% Simulation of the Controlled System - Solving the ODE
%
% 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, finalT); % solve the IPV described by F_ODE 
                           % in the time interval [t0, tf]

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

% the steady-state position
x_steady = (alphaP/(alphaP+k))*x0;

%
%
%% The Simulation Results: the Behavior of the Controlled System
%

% 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]);
yyaxis left
plot(t_stamps, cart_x(1,:), 'LineWidth',2); % cart position vs time
hold on; % let's add another plot: the set-point
plot(t_stamps, x0*ones(size(t_stamps)),'LineStyle','--','LineWidth',1.5); 
plot(t_stamps, x_steady*ones(size(t_stamps)),'LineStyle','-.','LineWidth',1.5); 
xlim([min(t_stamps), max(t_stamps)]);
xlabel('time $t$ [s]', 'FontSize', 14, 'Interpreter','latex'); % labels on the plot axes
ylabel('position $x$ [m]', 'FontSize', 14, 'Interpreter','latex'); 

yyaxis right
plot(t_stamps, cart_x(2,:), 'LineWidth',2); % cart speed vs time
ylabel('speed $\dot{x}$ [m/s]', 'FontSize', 14, 'Interpreter','latex'); 

grid on; % let's insert a grid in the figure

legend('cart position $x$', 'set point $x^{o}$', ...
        'Steady state position $x_{\infty}$', 'cart speed $\dot{x}$',...
        'Interpreter', 'latex', ...
        'FontSize', 14, 'Location', 'best' );% adding a legend

%
% Modify the simulation duration.
% Assign the set point.
% Modify the system parameters.
% Change the controller gains.
% Modify the initial conditions.
%
%% The ODE as MATLAB Function
%
% Let's define as a MATLAB function the first-order ODE system corresponding to
% \left\{ \begin{array}{rcl}
% \dot{y}_1(t) &=& y_2(t) \\
% \dot{y}_2(t) &=&-\frac{k+\alpha}{M}\,y_1(t) -\frac{h+\beta}{M}\,y_2(t) + \frac{\alpha}{M}\, x^{o}
% \end{array} \right.
% The MATLAB function must provide the first derivative vector \big[ \dot{y}_1(t),\;\dot{y}_2(t) \big]^{\top}, given the values of the functions y_1 and y_2 at the current time instant t and the parameters' values.
%

% 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 = CartPDFeedbackODE(t, y, p)
    % retrieving the parameters
    M_val      = p(1);
    h_val      = p(2);
    k_val      = p(3);
    x0_val     = p(4);
    alphaP_val = p(5);
    betaD_val  = p(6);

    kaM = (k_val+alphaP_val)/M_val;
    hM = (h_val+betaD_val)/M_val;
    U  = (alphaP_val/M_val)*x0_val;

    dydt = [y(2); ...
            -kaM*y(1)-hM*y(2)+U];

end % function

%
%
%
%
%
%
%
%