%% --------------------------------------------------------------------- %%
%  Modelling, Control, and Estimation of N-rotor vehicles
%
%% CONTINUOUS TIME SIMULATION EXERCISE 
%
%% --------------------------------------------------------------------- %%



%% ASSUMPTIONS:
%  1) The origin of the body frame is at the center of mass of the N-rotor
%     vehicle.
%  2) The thrusts from the propellers are all aligned with the positive
%     z-axis of the body frame.
%  3) Gravity is aligned with the negative z-axis of the inertial frame



%  ---------------------------------------------------------------------  %
%    CCCC  L      EEEEE    A    RRRR
%   C      L      E       A A   R   R
%   C      L      EEE    A   A  RRRR
%   C      L      E      AAAAA  R  R
%    CCCC  LLLLL  EEEEE  A   A  R   R
%  ---------------------------------------------------------------------  %
%% CLEAR AND CLOSE EVERYTHING
% > Clear all variables:
clear;
% > Close all open figures
close all;
% > Clear any text from the Command Window
clc;





%  ---------------------------------------------------------------------  %
%   V   V  EEEEE  H   H  III   CCCC  L      EEEEE
%   V   V  E      H   H   I   C      L      E
%   V   V  EEE    HHHHH   I   C      L      EEE
%    V V   E      H   H   I   C      L      E
%     V    EEEEE  H   H  III   CCCC  LLLLL  EEEEE
%
%   PPPP     A    RRRR     A    M   M  EEEEE  TTTTT  EEEEE  RRRR    SSSS
%   P   P   A A   R   R   A A   MM MM  E        T    E      R   R  S
%   PPPP   A   A  RRRR   A   A  M M M  EEE      T    EEE    RRRR    SSS
%   P      AAAAA  R  R   AAAAA  M   M  E        T    E      R  R       S
%   P      A   A  R   R  A   A  M   M  EEEEE    T    EEEEE  R   R  SSSS
%  ---------------------------------------------------------------------  %
%% PARAMETERS OF THE N-ROTOR VEHICLE

% INFO:
% The student should open the file "get_vehicle_paramters.m" and specify
% the N-rotor vehicle design as instructed in that file

exercise01_get_vehicle_paramters;

% The "get_vehicle_paramters_for_exercise01" script puts the following
% varibles into the workspace so that they can be used in the Simulink
% model:
% >> g
% >> g_vec
% >> nrotor_vehicle_mass_true
% >> nrotor_vehicle_mass_for_controller
% >> nrotor_vehicle_inertia_true
% >> nrotor_vehicle_inertia_true_inverse
% >> nrotor_vehicle_layout_true
% >> nrotor_vehicle_layout_for_controller
% >> nrotor_vehicle_thrust_min
% >> nrotor_vehicle_thrust_max
% >> nrotor_initial_condition



% INFO:
% The following function "visualise_nrotor_vehicle" makes a nice plot
% showing the top view layout of the N-rotor vehicle. Note that the rotor
% size is scaled automatically so that the circles representing each rotor
% "just touch", but this may sometimes lead to a strange appearance.
%if exist( 'visualise_nrotor_vehicle' , 'file' )
%    visualise_nrotor_vehicle( nrotor_vehicle_layout_true );
%end





%  ---------------------------------------------------------------------  %
%   DDDD   EEEEE  FFFFF    A    U   U  L      TTTTT
%   D   D  E      F       A A   U   U  L        T
%   D   D  EEE    FFF    A   A  U   U  L        T
%   D   D  E      F      AAAAA  U   U  L        T
%   DDDD   EEEEE  F      A   A   UUU   LLLLL    T
%
%    CCCC   OOO   N   N  TTTTT  RRRR    OOO   L      L      EEEEE  RRRR
%   C      O   O  NN  N    T    R   R  O   O  L      L      E      R   R
%   C      O   O  N N N    T    RRRR   O   O  L      L      EEE    RRRR
%   C      O   O  N  NN    T    R  R   O   O  L      L      E      R  R
%    CCCC   OOO   N   N    T    R   R   OOO   LLLLL  LLLLL  EEEEE  R   R
%  ---------------------------------------------------------------------  %
%% LOAD THE HARD-CODED "DEFAULT" LINEAR QUADRATIC REGULATOR (LQR) CONTROLLER

% INFO:
% > For the purposes of the template, an LQR controller feedback matrix is
%   hard-coded and should stabilise most N-rotor vehicle layouts.

exercise01_get_default_controller;

% The "get_vehicle_paramters_for_exercise01" script puts the following
% varibles into the workspace so that they can be used here:
% >> K_lqr_full_state_continous_time
% >> K_lqr_full_state_discrete_time_20Hz
% >> K_lqr_full_state_discrete_time_50Hz
% >> K_lqr_full_state_discrete_time_100Hz
% >> K_lqr_full_state_discrete_time_200Hz


% SELECT HERE THE CONTROLLER TO USE BY UN-COMMENTING THE DESIRED LINE
K_lqr_full_state = K_lqr_full_state_continous_time;
%K_lqr_full_state = K_lqr_full_state_discrete_time_20Hz;
%K_lqr_full_state = K_lqr_full_state_discrete_time_50Hz;
%K_lqr_full_state = K_lqr_full_state_discrete_time_100Hz;
%K_lqr_full_state = K_lqr_full_state_discrete_time_200Hz;





%  ---------------------------------------------------------------------  %
%    SSSS  TTTTT  U   U  DDDD   EEEEE  N   N  TTTTT
%   S        T    U   U  D   D  E      NN  N    T
%    SSS     T    U   U  D   D  EEE    N N N    T
%       S    T    U   U  D   D  E      N  NN    T
%   SSSS     T     UUU   DDDD   EEEEE  N   N    T
%
%    CCCC   OOO   N   N  TTTTT  RRRR    OOO   L      L      EEEEE  RRRR
%   C      O   O  NN  N    T    R   R  O   O  L      L      E      R   R
%   C      O   O  N N N    T    RRRR   O   O  L      L      EEE    RRRR
%   C      O   O  N  NN    T    R  R   O   O  L      L      E      R  R
%    CCCC   OOO   N   N    T    R   R   OOO   LLLLL  LLLLL  EEEEE  R   R
%  ---------------------------------------------------------------------  %
%% THIS EXERCISE DOES NOT REQUIRE THE STUDENT TO DESIGN A CONTROLLER

% > For students implementing their own LQR controller, they should add
%   the necessary code to define the linearisation of the N-rotor vehicle
%   and solve the Riccati equation to get their own LQR controller feedback
%   matrix.





%  ---------------------------------------------------------------------  %
%   EEEEE   QQQ   U   U  III  L      III  BBBB   RRRRI  III  U   U  M   M
%   E      Q   Q  U   U   I   L       I   B   B  R   R   I   U   U  MM MM
%   EEE    Q   Q  U   U   I   L       I   BBBB   RRRR    I   U   U  M M M
%   E      Q  Q   U   U   I   L       I   B   B  R  R    I   U   U  M   M
%   EEEEE   QQ Q   UUU   III  LLLLL  III  BBBB   R   R  III   UUU   M   M
%
%   TTTTT  H   H  RRRR   U   U   SSSS  TTTTT   SSSS
%     T    H   H  R   R  U   U  S        T    S
%     T    HHHHH  RRRR   U   U   SSS     T     SSS
%     T    H   H  R  R   U   U      S    T        S
%     T    H   H  R   R   UUU   SSSS     T    SSSS
%  ---------------------------------------------------------------------  %
%% COMPUTE THE EQUILIBRIUM THRUSTS

% INFO:
% The "exercise01_compute_equilibrium_thrusts" function computes and
% returns the equilibrium thrusts as a column vector of length N. Thus the
% variable "nrotor_vehicle_equilibrium_thrust" is in the workspace and can
% be used in the Simulink model.

nrotor_vehicle_equilibrium_thrust = exercise01_compute_equilibrium_thrusts(nrotor_vehicle_layout_for_controller,nrotor_vehicle_mass_for_controller,g);






%  ---------------------------------------------------------------------  %
%   RRRR   EEEEE  FFFFF  EEEEE  RRRR   EEEEE  N   N   CCCC  EEEEE
%   R   R  E      F      E      R   R  E      NN  N  C      E
%   RRRR   EEE    FFFF   EEE    RRRR   EEE    N N N  C      EEE
%   R  R   E      F      E      R  R   E      N  NN  C      E
%   R   R  EEEEE  F      EEEEE  R   R  EEEEE  N   N   CCCC  EEEEE
%  ---------------------------------------------------------------------  %
%% SPECIFY THE REFERENCE SIGNAL FOR (x,y,z,yaw)

% INFO:
% The student can open the file
% "get_reference_specification_for_exercise01.m" and follow the comments
% there to adjust the reference signal
exercise01_get_reference_specification;


% The "get_reference_specification_for_exercise01" script puts the
% following varibles into the workspace so that they can be used in the
% Simulink model:
% >> reference_step_xyz_yaw
% >> reference_step_period
% >> reference_sine_amplitude_xyz_yaw
% >> reference_sine_bias_xyz_yaw
% >> reference_sine_period
% >> reference_saw_tooth_amplitude_xyz_yaw
% >> reference_saw_tooth_bias_xyz_yaw
% >> reference_step_inclusion_multiplier
% >> reference_sine_wave_inclusion_multiplier
% >> reference_saw_tooth_inclusion_multiplier

