% Matlab Init                                              :noexport:ignore:

%% test_nhexa_table.m

%% Clear Workspace and Close figures
clear; close all; clc;

%% Intialize Laplace variable
s = zpk('s');

%% Path for functions, data and scripts
addpath('./mat/'); % Path for Data
addpath('./src/'); % Path for functions
addpath('./STEPS/'); % Path for STEPS
addpath('./subsystems/'); % Path for Subsystems Simulink files

%% Initialize Parameters for Simscape model
table_type = 'Rigid'; % On top of vibration table
device_type = 'None'; % On top of vibration table
payload_num = 0; % No Payload

% Simulink Model name
mdl = 'test_bench_nano_hexapod';

%% Colors for the figures
colors = colororder;

%% Frequency Vector
freqs = logspace(log10(10), log10(2e3), 1000);

% Simscape Model of the suspended table
% <<ssec:test_nhexa_table_model>>

% The Simscape model of the suspended table simply consists of two solid bodies connected by 4 springs.
% The 4 springs are here modelled with "bushing joints" that have stiffness and damping properties in x, y and z directions.
% The 3D representation of the model is displayed in Figure ref:fig:test_nhexa_suspended_table_simscape where the 4 "bushing joints" are represented by the blue cylinders.

% #+name: fig:test_nhexa_suspended_table_simscape
% #+caption: 3D representation of the simscape model
% #+attr_latex: :width 0.8\linewidth
% [[file:figs/test_nhexa_suspended_table_simscape.png]]

% The model order is 12, and it represents the 6 suspension modes.
% The inertia properties of the parts are set from the geometry and material densities.
% The stiffness of the springs was initially set from the datasheet nominal value of $17.8\,N/mm$ and then reduced down to $14\,N/mm$ to better match the measured suspension modes.
% The stiffness of the springs in the horizontal plane is set at $0.5\,N/mm$.
% The obtained suspension modes of the simscape model are compared with the measured ones in Table ref:tab:test_nhexa_suspended_table_simscape_modes.


%% Configure Simscape Model
table_type = 'Suspended'; % On top of vibration table
device_type = 'None'; % No device on the vibration table
payload_num = 0; % No Payload

%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/F'],      1, 'openinput');  io_i = io_i + 1;
io(io_i) = linio([mdl, '/F_v'],  1, 'openoutput'); io_i = io_i + 1;

%% Run the linearization
G = linearize(mdl, io);
G.InputName  = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
G.OutputName  = {'Vdx', 'Vdy', 'Vdz', 'Vrx', 'Vry', 'Vrz'};

%% Compute the resonance frequencies
ws = eig(G.A);
ws = ws(imag(ws) > 0);