%% ELEC-C1310: Laboratory exercises in Automation and Control Engineering - Pre-assignment
%
% ====> Student Name:
% ====> Student Number:
%
% This assignment is designed to deepen your understanding of control theory principles, specifically through the design and analysis of Proportional-Derivative (PD) and Linear Quadratic Regulator (LQR). Completing this assignment will prepare you for the upcoming lab sessions.
% PD Controller Design: You will explore the dynamics of a control system response to input signals and learn how to manipulate PD controller parameters to achieve desired system behavior, such as specific overshoot and settling time.
% LQR Design: You will apply the LQR control strategy to optimize the performance of a Rotary Inverted Pendulum model.
%
%%  Task 1: PD Controller Design

clear; clc;

%
%% Introduction
% The Qube Servo 2 voltage to position transfer function  \( G(s) \) can be defined as:
% G(s) = \frac{\Theta_m(s)}{V_m(s)} =\frac{K}{s(\tau s + 1)} = \frac{23.2}{s(0.13s + 1)},
% where we use a steady-state gain \( K = 23.2 \, \text{rad/(V-s)} \) and a time constant \( \tau = 0.13 \, \text{s} \). constant. In this system, we have \Theta_m(s) as the motor/disk position and V_m(s) as the applied motor voltage [1].
% In the lab session, you will focus on the PID controllers and the effect of each term on the closed loop system. Before that, we have an analytical design pre-assignment. In this assignment, you will focus on the PD controllers for the simplicity.
% Given the plant transfer function
% \[ G(s) = \frac{K}{\tau s^2 + s} \]
% and the PD controller
% \[ C(s) = K_p + K_d s .\]
%
% Fig. 1 - PD Controlled Closed-Loop System
% Then, the closed-loop transfer function T_{cl}(s) can be written after a set of calculations by using intermediate signals. Let's start with the controller's output which is determined by the proportional and derivative actions on the error signal:
% $$
% U(s) = (K_p + K_d s) E(s).
% $$
% Plant output as a function of the controller's output is
% $$
% Y(s) = \frac{K}{(\tau s + 1)s} U(s).
% $$
% The error signal derived from the reference input and plant's output is
% $$
% E(s) = R(s) - Y(s).
% $$
% Substituting the expressions for U(s) and E(s) into Y(s) yields
% $$
% Y(s) = \frac{K}{(\tau s + 1)s} (K_p + K_d s)(R(s) - Y(s)).
% $$
% Rearranging terms to isolate Y(s):
% $$
% (\tau s^2 + s) Y(s) + K (K_p + K_d s) Y(s) = K (K_p + K_d s) R(s).
% $$
% After rearranging the terms, we aim to isolate the output Y(s) on one side to solve for the closed-loop transfer function. The final equation for Y(s) in terms of R(s) is
% $$
% Y(s) (\tau s^2 + s + K K_p + K K_d s) = K (K_p + K_d s) R(s).
% $$
% The final form of the closed-loop transfer function is
% $$
% T_{cl}(s) = \frac{Y(s)}{R(s)} = \frac{K (K_p + K_d s)}{\tau s^2 + s (1 + K K_d) + K K_p}.
% $$
% This is the closed-loop transfer function, \( T_{cl}(s) \), which defines the system's behavior from input to output. The characteristic equation of the closed-loop system \[ \lambda_{cl}(s)
%  is found by setting denominator of \( T_{cl}(s) = 0\):
% \[ \lambda_{cl}(s) =\tau s^2 + (1 + KK_d) s + KK_p = 0. \]
%
%% Second Order System Characterization
% Now, we will explain our design idea. For that we will recall the generic second order system characterization. Second-order systems are characterized by a transfer function with a quadratic denominator in the standard form:
% \[ T_{des}(s) = \frac{\omega_n^2}{s^2 + 2\zeta\omega_n s + \omega_n^2} \]
% where \( \omega_n \) is the natural frequency, and \( \zeta \) is the damping ratio. The natural frequency \( \omega_n \) indicates the system's responsiveness, while the damping ratio \( \zeta \) characterizes the oscillatory nature of the system's response. The desired characteristic equation of the system is:
% \[\lambda_{des}(s) = s^2 + 2\zeta\omega_n s + \omega_n^2 = 0. \]
% The roots of this equation, or the poles of the transfer function \( T_{des}(s) \), determine the system's response characteristics. The location of the poles in the complex plane provides insights into the transient and steady-state behaviors of the system. For or PD-controlled system, the controller parameters \( K_p \) and \( K_d \) are adjusted such that the closed-loop system behaves like a desired second-order system with specified \( \zeta \) and \( \omega_n \) values.
% So far we,
% Derived the closed loop transfer function T_{cl}(s) parametrically.
% Switched to its characteristic equation \lambda_{cl}(s).
% Observed that \lambda_{cl}(s) is of the second order.
% Presented the generic form of a second-order system's transfer function as the desired closed-loop transfer function T_{des}(s). This form includes \( \zeta \) and \( \omega_n \)which are viewed as design parameters.
% Switched to a desired characteristic equation to \[\lambda_{des}(s)
% .
% Next steps,
% Given the desired system requirements, we can compute \( \zeta \)and \( \omega_n \).
% Then, we can calculate the T_{des}(s) and \[\lambda_{des}(s)
% .
% Finally, we can set the equality  \lambda_{des}(s) = \lambda_{cl}(s) to find the PD-controller parameters.
% At the end, we can check the step response of the system if the desired system requirements are satisfied.
%
%% Control Requirements to Design Parameters - Related to Question 1
% In many cases, we may look for spesific steady-state or transient state criterias. Let's say we would like to design a controller such that the closed-loop system will have 10\% Overshoot (PO) and 0.35 seconds of settling time (T_s).
% The formula for PO in terms of the damping ratio \(\zeta\) is given by:
% \[ PO = 100 e^{\left(\frac{-\zeta\pi}{\sqrt{1-\zeta^2}}\right)} \]
% If we arrange the terms then we will conclude that \(\zeta\) is given by:
% \zeta = \frac{-\ln\left(\frac{PO}{100}\right)}{\sqrt{\pi^2 + \ln^2\left(\frac{PO}{100}\right)}}
% The formula for \(T_s\), assuming a 2\% criterion, in terms of the \(\zeta\) and \(\omega_n\) is:
% \[ T_s = \frac{4}{\zeta \omega_n} \]
% If we arrange the terms then we will conclude that \(\omega_n\) is given by:
% \[ \omega_n= \frac{4}{\zeta T_s } \]
% Question 1) Follow the instructions given by the comments. Fill the below code chunk and find the \(\zeta\)and \(\omega_n\) which satisfies the PO =10\% and $T_s=0.35$ .

% Given Percentage Overshoot (PO) and Settling Time (Ts)
PO = 10;
Ts = 0.35;

% Convert PO to a decimal for the formula
PO_decimal = PO / 100;

%%%%%%%%%%%%%%%%%%%%%%%
%%% YOUR CODE HERE %%%%
%%%%%%%%%%%%%%%%%%%%%%%
% Calculate Damping Ratio (zeta) using the percentage overshoot formula
% (use PO_decimal)

% zeta = ?

% Calculate Natural Frequency (wn) using the settling time formula

% wn =?

%%%%%%%%%%%%%%%%%%%%%%%
%%% YOUR CODE HERE %%%%
%%%%%%%%%%%%%%%%%%%%%%%

% Let's print out the values:

% Display the calculated values
fprintf('Calculated Damping Ratio (zeta): %.4f\n', zeta);
fprintf('Calculated Natural Frequency (wn): %.4f rad/s\n', wn);

%
%% Find the Controller Parameters - Related to Question 2
% Please read carefully and understand the derivations. Then solve the Question 2.
% We have identified the design parameters according to design criterias. We can set   \lambda_{cl}(s) =  \lambda_{des}(s)  and solve for our parameters since we have derived \(\zeta\) and \(\omega_n\).
% Recall that our \lambda_{cl}(s) :
% \[ \lambda_{cl}(s) = \tau s^2 + (1 + KK_d) s + KK_p = 0 \]
% Let's normalize the leading coefficient:
% \[ \lambda_{cl}(s) =  s^2 + \frac{1 + KK_d}{\tau} s + \frac{KK_p}{\tau} = 0 \]
% Recall that our \lambda_{des}(s):
% \[\lambda_{des}(s) = s^2 + 2\zeta\omega_n s + \omega_n^2 = 0 \]
% Then, we have following equality for s term:
%  \frac{1 + KK_d}{\tau} = 2\zeta\omega_n
% And for the constant term:
% \frac{KK_p}{\tau} =  \omega_n^2
% Question 2) Follow the instructions given by the comments. Fill the below code chunk and find the \(K_p\)and K_d. Use solve function. (Please take a look at the MATLAB Help link.)
%

% Our symbolic variables
syms Kd Kp

% System parameters
K = 23.2;
tau = 0.13;

%%%%%%%%%%%%%%%%%%%%%%%
%%% YOUR CODE HERE %%%%
%%%%%%%%%%%%%%%%%%%%%%%

% For Kd, first define the equality then solve for Kd

% equation_for_Kd = ? == ?;
% Kd_solution = ?;

% For Kd, first define the equality then solve for Kd

% equation_for_Kp = ? == ?;
% Kp_solution = ?;

%%%%%%%%%%%%%%%%%%%%%%%
%%% YOUR CODE HERE %%%%
%%%%%%%%%%%%%%%%%%%%%%%

% Let's print out our controller parameters:

% Implement the PD controller with solved values
Kp_val = double(Kp_solution);  % Convert symbolic solution to numeric value
Kd_val = double(Kd_solution);  % Convert symbolic solution to numeric value
fprintf('Kp: %.4f\n', Kp_val);
fprintf('Kd: %.4f\n', Kd_val);

%
%% Check the Step Response for Closed Loop System - Related to Question 3 / Question 4
% Now, we have performed our symbolic calculations and derived the controller parameters according to the design criterias. We can test our results and check that if it is satisfied under the step response.
% Question 3) Create the closed loop transfer function. To do that, you need to define:
% Define the PD controller by using the pid function. You just need to use the parameters that you computed and set the Integral term to 0. (Referred as C(s))
% Define the servo motor model by using the tf function. (Referred as G(s))
% Then, you will use feedback function to create the cloosed loop transfer function T_{cl}(s). (Hint: We have unit feedback.)

%%%%%%%%%%%%%%%%%%%%%%%
%%% YOUR CODE HERE %%%%
%%%%%%%%%%%%%%%%%%%%%%%

% Define the PD controller in MATLAB

% Cpd_s = ;

% Define the system G(s)

% G_s = ;

% Closed-loop system

% Tcl_s = ;

%%%%%%%%%%%%%%%%%%%%%%%
%%% YOUR CODE HERE %%%%
%%%%%%%%%%%%%%%%%%%%%%%

% Let's check the step response.

% Plot the step response
step(Tcl_s);
title('Closed-Loop Step Response with Designed PD Controller');
grid on;

%
% See if it is possible to satisfy the criteria:

properties = stepinfo(Tcl_s)
Ts = properties.SettlingTime
PO = properties.Overshoot

%
%% Comment on the Results - Related to Question 4
%
% Question 4) Comment on the achieved results.
% Was the expected Overshoot < 10%  criteria satisfied? If not, try to explain what might be caused to this?
% Write your answer:
% Was the expected Ts < 0.35 criteria satisfied? If not, try to explain what might be caused to this?
% Write your answer:
%
%% Task 2: LQR Design
%
%% Linearization
% Please go through the derivation and linearization of the EOMs (Equations of Motion) in Section 3 of the provided pre-assignment description file and answer the following questions related to linearization.
%
% Question 1) Explain why the non-linear model is linearized in the context of the LQR controller.
%
% Your answer:
%
% Question 2) How is the linearization done? (Hint: Approximate the functions f(\alpha) = \sin(\alpha) and g(\alpha) = \cos(\alpha) as the first two terms of the Taylor series. Note that the linearized model only works properly when is close to 0, so the linearization is done at the real value 0.)
%
% Your answer:
%
%% State Space Representation
% After we have the linearized the EOMs, the state space model can be formed by solving the equations for acceleration [1]. The equations then become
% \ddot{\theta} = \frac{1}{J_t}\left(m_p^2l^2rg\alpha - J_pb_r\dot{\theta} -m_plrb_p\dot{\alpha} + J_p\tau\right)
% and
% \ddot{\alpha} = \frac{1}{J_t}\left(m_pglJ_r\alpha - m_plrb_r\dot{\theta} - J_pb_p\dot{\alpha} + m_prl\tau\right),
% where
% J_t = J_pJ_r - m_p^2l^2r^2.
%
% The linear state-space model can now be formed. The linear state-space model is of the form
% \dot{x}(t) = Ax(t) + Bu(t)
% where
% \dot{x}(t) = \left[ \begin{array}{cc} \dot\theta(t) & \dot\alpha(t) & \ddot{\theta}(t) & \ddot{\alpha}(t) \end{array} \right]^T
% and
% x(t) = \left[ \begin{array}{cc} \theta(t) & \alpha(t) & \dot{\theta}(t) & \dot{\alpha}(t) \end{array} \right]^T
%
% Based on the equations above, the state-space matrices A and B become
% \[
% A = \left[ \begin{array}{cccc}
% 0 & 0 & 1 & 0 \\
% 0 & 0 & 0 & 1 \\
% 0 & \frac{m_p^2l^2rg}{J_t} & -\frac{b_rJ_p}{J_t} & -\frac{m_plrb_p}{J_t} \\
% 0 & \frac{m_pglJ_r}{J_t} & -\frac{m_plrb_r}{J_t} & -\frac{J_pb_p}{J_t}
% \end{array} \right]
% \]
% and
% B = \left[ \begin{array}{c} 0 \\ 0 \\ \frac{J_p}{J_t} \\ \frac{m_p \cdot l \cdot r}{J_t} \end{array} \right].
% The state-space model is defined in the code below, so that the optimal K  matrix can be calculated using Matlab's LQR function.
%% Motor Parameters
% The parameters such as motor resistance or pendulum length are given by Quanser in the code below:

%% Motor
% Resistance
Rm = 8.4;
% Current-torque (N-m/A)
kt = 0.042;
% Back-emf constant (V-s/rad)
km = 0.042;
%
%% Rotary Arm
% Mass (kg)
mr = 0.095;
% Total length (m)
r = 0.085;
% Moment of inertia about pivot (kg-m^2)
Jr = mr*r^2/3;
% Equivalent Viscous Damping Coefficient (N-m-s/rad)
br = 1e-3; % damping tuned heuristically to match QUBE-Sero 2 response
%
%% Pendulum Link
% Mass (kg)
mp = 0.024;
% Total length (m)
Lp = 0.129;
% Pendulum center of mass (m)
l = Lp/2;
% Moment of inertia about pivot (kg-m^2)
Jp = mp*Lp^2/3;

% Equivalent Viscous Damping Coefficient (N-m-s/rad)
bp = 5e-5; % damping tuned heuristically to match QUBE-Sero 2 response
% Gravity Constant
g = 9.81;

% Find Total Inertia
Jt = Jr*Jp - mp^2*r^2*l^2;

%
%% Define the State Space Matrices for LQR Control
% Note that the actuator dynamics are added so that the input is the motor voltage v_m instead of torque \tau.

% State Space Representation
A = [0 0 1 0;
     0 0 0 1;
     0 mp^2*l^2*r*g/Jt  -br*Jp/Jt   -mp*l*r*bp/Jt 
     0  mp*g*l*Jr/Jt    -mp*l*r*br/Jt   -Jp*bp/Jt];

B = [0; 0; Jp/Jt; mp*l*r/Jt];
C = eye(2,4); 
D = zeros(2,1); 

% Add actuator dynamics
% Transform torque into motor voltage
A(3,3) = A(3,3) - km*km/Rm*B(3);
A(4,3) = A(4,3) - km*km/Rm*B(4);
B = km * B / Rm;

% Get the open loop system
sys = ss(A,B,C,D)

% Let's define Q and R matrices:

% Set Q matrix to identity and R to 1
Q = eye(4);
R = 1;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
% Remember to change these in task 5 %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Q(1,1) = 1;
Q(2,2) = 1;
Q(3,3) = 1;
Q(4,4) = 1;
Q % show Q matrix 

%
% Question 3) What are the purposes of the Q and R matrices?
% Your answer:
%
%
% Question 4) Given the state-space matrices, use MATLAB's lqr function to calculate the optimal gain matrix K.

%%%%%%%%%%%%%%%%%%%%%%%
%%% YOUR CODE HERE %%%%
%%%%%%%%%%%%%%%%%%%%%%%

% Solution %

% K_std = ?

%
% Question 5) Plot the step response with the code below. Then, change the first element (Q(1,1)) of the Q-matrix to 20. See what changes, and explain why.(Please write your answer below of the plot.)
% We encourage you to also try different values and see how the step response changes.

% Closed loop model 
A_cl = A - B*K_std;

states = {'theta', 'alpha',  'thetadot', 'alphadot'};
outputs = {'theta'; 'alpha'};

% Closed loop system
sys_cl = ss(A_cl,B,C,D,'statename',states,'outputname',outputs);

% Define 5 second time axis
t = 0:0.01:5;

% Plot the step response
step(sys_cl, t)
grid on;

% --- Your answer to Question 5:
%
%
%
%% References
% [1] J. Apkarian and M. Lévis, QUBE-Servo Experiment for MATLAB /Simulink Users Student Workbook. Markham, Ontario, Canada: Quanser Inc., 2014.