%% Simulation with ODE45 of a closed-loop Proportional+Derivative control of an unstable pendulum
% © 2022, Antonio Sala Piqueras. Universitat Politècnica de València. All rights reserved.
% This code successfully ran on Matlab R2021b (Linux)
% Presentation in video: http://personales.upv.es/asala/YT/V/bcode45EN.html
% Objectives: Understanding how to simulate a closed loop (plant+controller), assuming no algebraic loops, using ode45, without the need of Simulink's GUI.
% Table of Contents
% Modelling
%     Process to be controlled
%     Linear PD Controller
%          Controller State Equation
%         Controller Output Equation
% Numerical simulation
% Plots
%     Comparison with linearisation (simulated by control systems toolbox)
% Auxiliary Functions (closed loop state equation)
%
%% Modelling
%% Process to be controlled
% A nonlinear model of a pendulum is:
% \frac{dp}{dt}=v,          \frac{dv}{dt}=\frac{g}{l^2}\cdot \sin(p)-\frac{f}{Ml^2}\cdot v+\frac{1}{Ml^2}T
% being T the input torque, used as manipulated variable here.

M=0.5;g=9.81; fric=0.25; l=1;%constant parameters

% The controller will be linear, so we need to specify the operating point in order to compute the changes to/from incremental variables:

pt_oper_y=0; %upright equilibrium
pt_oper_T=0; %Theoretically, no need of force there

% It's just by chance that "zero" in incrementals corresponds to zero in absolute units, this is not true in a general case, of course.
% Nonlinear state-space representation:

Model.Process.StateEq=@(t,x,T) [x(2); g*sin(x(1))-fric/M*x(2)+T/M ];
Model.Process.OutputEq=@(t,x) x(1); %measurement available for feedback
ProcessOrder=2;

%% Linear PD Controller
% We will use a hand-tuned PD controller (with derivative noise filter):

s=tf('s');
Kp=7;Kd=1.3;filterbandwith=20;
KLaplace=Kp+Kd*s/(s/filterbandwith+1);
K=ss(KLaplace)
ControllerOrder=size(K.A,1)

%%  Controller State Equation

Model.Controller.StateEq=@(t,x,y) K.A*x+K.B*(-(y-pt_oper_y)); % negative feedback

% With negative feedback, controller input is -\Delta y = -(y-pt_oper_y), i.e., the same as (pt_oper_y-y), the loop error if "output operating point" is considered to be "setpoint".
%% Controller Output Equation
% When controlling a nonlinear process, the incremental output of a linear controller must be converted to absolute output (adding input setpoint), saturated, etc., so output equation is not just T=K.C*x+K.D*(-incr_y). The modifications are:

Limit_T_sup=3.5;Limit_T_inf=-3.5; %saturation limits
saturate=@(T) max(min(T,Limit_T_sup),Limit_T_inf); %saturation function

Model.Controller.OutputEq= ...
    @(t,x,y) saturate(pt_oper_T + K.C*x+K.D*(-(y-pt_oper_y)));
% Note that antiwindup would be needed if controller incorporates integral
% action. Not needed here in this simplistic simulation.

%%
%% Numerical simulation
% We'll set up the format ode45 needs for working: a single state vector juxtaposing process and controller states.

IndexStatesProcess=1:ProcessOrder
IndexStatesControl=ProcessOrder+(1:ControllerOrder)
odefun= @(t,FullState) ...
  CL_StateEQ(t,FullState(IndexStatesProcess),FullState(IndexStatesControl),Model);

%
% We'll set initial conditions

StateIniProcess=[pi/30;0]; %test pi/30 (no saturation), pi/10 (difference between linear/nonlinear), pi/4 (almost falls), pi/2 (falls)
StateIniController=0;
x0=[StateIniProcess;StateIniController];% initial conds. Process+regulator

% And actual simulation (numerical integration) is readily done in one line with ode45:

t_final=6;
[T,X]=ode45(odefun, [0 t_final], x0, odeset('RelTol',1e-6,'AbsTol',1e-6));

% Finished.
%% Plots
% The output of ode45 is the state vector.

plot(T,X(:,IndexStatesProcess),LineWidth=2)
title("States (Process)"), legend("Pend. Position","Velocity")
grid on

% Outputs are usually the most meaningful variable. Let us plot them:

Ns=length(T);
y=zeros(1,Ns); u=zeros(1,Ns);
for k=1:Ns
    y(k)=Model.Process.OutputEq(T(k),X(k,IndexStatesProcess)'); %Outputs from Process
    u(k)=Model.Controller.OutputEq(T(k),X(k,IndexStatesControl)',y(k)); %Outputs from Controller
end

plot(T,y,'k',LineWidth=3)
grid on
title("Process Output (y)")
plot(T,u,LineWidth=2)
grid on
title("Controller Output (u)")

%% Comparison with linearisation (simulated by control systems toolbox)
% Linearised Model
% \frac{d\bar p}{dt}=\bar v,           \frac{d\bar v}{dt}=\frac{g}{l^2}\cdot \cos(p_0)\bar p-\frac{f}{Ml^2}\cdot \bar v+\frac{1}{Ml^2}\bar T

A=[0 1; cos(pt_oper_y)*g/l^2 -fric/M/l^2]; B=[0; 1/M/l^2]; C=[1 0]; %D=0
sysLin=ss(A,B,C,0); %for control system toolbox, just to compare... not needed in our ode45 simulation, of course

% Closing the loop is done with:

BC=feedback(sysLin,K);
pole(BC)
plot(T,y,Color=[.6 .6 .6],LineWidth=3), hold on %nonlinear simulation

% This is the linear simulation by Control Systems Toolbox:

initial(BC,x0-[pt_oper_y;0;0],t_final), grid on, hold off,legend("ode45","Control Syst. Toolbox (lin)",location="best")

%
%% Auxiliary Functions (closed loop state equation)

function dxdtbc=CL_StateEQ(t,ProcessState,ControlState, M)
%We assume controller has dynamics, otherwise, see note 2.
  Measurements=M.Process.OutputEq(t,ProcessState); %D=0, no algebraic loop, please
  dControllerStatedt=M.Controller.StateEq(t,ControlState,Measurements);
  u=M.Controller.OutputEq(t,ControlState,Measurements);
  dSProcesStatedt=M.Process.StateEq(t,ProcessState,u);
  dxdtbc=[dSProcesStatedt;dControllerStatedt];
end

% *Note 1: If Measurements depended on "u", then there would be an algebraic loop. We assume that situation does not occur, otherwise this code would be incorrect.
% *Note 2: If controller were just a "proportional" controller, there would be no need of M.Controller.StateEq; we could spare the "ControlState" argument above, and replace just to
%  u=M.Controller.OutputEq(t,Measurements)
% It is left to the reader to change the code to simulate how a proportional controller would work (note that it will not be a stabilising one, as a root locus analysis with the linearised model would easily prove).
%
%
%
%
%