diff --git a/.SimulinkProject/Root.type.Files/Assemblage_DataFile.m.type.File.xml b/.SimulinkProject/Root.type.Files/Assemblage_DataFile.m.type.File.xml
deleted file mode 100644
index 80b5b16..0000000
--- a/.SimulinkProject/Root.type.Files/Assemblage_DataFile.m.type.File.xml
+++ /dev/null
@@ -1,6 +0,0 @@
-
-
-
-
-
-
\ No newline at end of file
diff --git a/.SimulinkProject/Root.type.ProjectPath/2793b451-85f9-44a8-8d10-25d7657da4e0.type.Reference.xml b/.SimulinkProject/Root.type.ProjectPath/2793b451-85f9-44a8-8d10-25d7657da4e0.type.Reference.xml
new file mode 100644
index 0000000..6fb784a
--- /dev/null
+++ b/.SimulinkProject/Root.type.ProjectPath/2793b451-85f9-44a8-8d10-25d7657da4e0.type.Reference.xml
@@ -0,0 +1,2 @@
+
+
\ No newline at end of file
diff --git a/.SimulinkProject/Root.type.ProjectPath/74f6c052-8691-4256-bd9d-7bbb44c7ca82.type.Reference.xml b/.SimulinkProject/Root.type.ProjectPath/74f6c052-8691-4256-bd9d-7bbb44c7ca82.type.Reference.xml
new file mode 100644
index 0000000..d6a3306
--- /dev/null
+++ b/.SimulinkProject/Root.type.ProjectPath/74f6c052-8691-4256-bd9d-7bbb44c7ca82.type.Reference.xml
@@ -0,0 +1,2 @@
+
+
\ No newline at end of file
diff --git a/.SimulinkProject/Root.type.ProjectPath/c767e49c-8d99-42c0-ab3b-d872f82e48d2.type.Reference.xml b/.SimulinkProject/Root.type.ProjectPath/c767e49c-8d99-42c0-ab3b-d872f82e48d2.type.Reference.xml
new file mode 100644
index 0000000..0c381b2
--- /dev/null
+++ b/.SimulinkProject/Root.type.ProjectPath/c767e49c-8d99-42c0-ab3b-d872f82e48d2.type.Reference.xml
@@ -0,0 +1,2 @@
+
+
\ No newline at end of file
diff --git a/Assemblage.slx b/Assemblage.slx
index 09c0f5a..31b7f7f 100644
Binary files a/Assemblage.slx and b/Assemblage.slx differ
diff --git a/Control/control_y.m b/Control/control_y.m
index 729a3d1..15abb00 100644
--- a/Control/control_y.m
+++ b/Control/control_y.m
@@ -1,55 +1,34 @@
-%% Define options for bode plots
-bode_opts = bodeoptions;
-
-bode_opts.Title.FontSize = 12;
-bode_opts.XLabel.FontSize = 12;
-bode_opts.YLabel.FontSize = 12;
-bode_opts.FreqUnits = 'Hz';
-bode_opts.MagUnits = 'abs';
-bode_opts.MagScale = 'log';
-bode_opts.PhaseWrapping = 'on';
-bode_opts.PhaseVisible = 'on';
%%
-load('../mat/G_f_to_d.mat', 'G_f_to_d');
+load('./mat/G_f_to_d.mat', 'G_f_to_d');
%%
G = G_f_to_d(2, 2);
-%% Some post processing of the plant
-[G, ~] = freqsep(G, 2*pi*1000);
-[~, G] = freqsep(G, 2*pi*1);
-
-%% Verify the post processing
-figure;
-bode(G, G_f_to_d(2, 2));
-
%% Try sisotool
sisotool(G)
%%
-gain = 1e8;
+gain = 1e9;
%%
-figure;
-bode(gain*G, bode_opts)
+bodeFig({gain*G}, struct('phase', true))
%%
[~,~,~,Wpm] = margin(gain*G);
-% Wpm = 180*2*pi;
+Wpm = 200*2*pi;
%%
s = tf('s');
-Ky = gain*(s/(0.2*Wpm)+1)/(s/(10*Wpm)+1)/(1+s/(2*pi*100));%*(s+2*pi*10)/(s+2*pi*0.0001);
+C = gain*(s/(0.2*Wpm)+1)/(s/(10*Wpm)+1)/(1+s/(2*pi*100));%*(s+2*pi*10)/(s+2*pi*0.0001);
%% Compute Closed loop transfer function
-figure;
-bode(Ky*G, bode_opts)
+bodeFig({C*G}, struct('phase', true))
%%
K = tf(zeros(6));
-K(2,2) = Ky;
+K(2,2) = C;
%%
-save('../mat/controller.mat', 'K')
+save('./mat/controller.mat', 'K')
diff --git a/Identification/identification_control.m b/Identification/identification_control.m
index e040445..2776753 100644
--- a/Identification/identification_control.m
+++ b/Identification/identification_control.m
@@ -1,25 +1,11 @@
%% Script Description
%
%%
-clear;
-close all;
-clc
-
-%% Define options for bode plots
-bode_opts = bodeoptions;
-
-bode_opts.Title.FontSize = 12;
-bode_opts.XLabel.FontSize = 12;
-bode_opts.YLabel.FontSize = 12;
-bode_opts.FreqUnits = 'Hz';
-bode_opts.MagUnits = 'abs';
-bode_opts.MagScale = 'log';
-bode_opts.PhaseWrapping = 'on';
-bode_opts.PhaseVisible = 'on';
+clear; close all; clc;
%% Options for preprocessing the identified transfer functions
f_low = 10;
-f_high = 1000;
+f_high = 10000;
%% Options for Linearized
options = linearizeOptions;
@@ -28,21 +14,23 @@ options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'Assemblage';
-%% Y-Translation Stage
+%% NASS
% Input/Output definition
-io(1) = linio([mdl, '/Fnass_cart'],1,'input');
-io(2) = linio([mdl, '/Sample'],1,'output');
+io(1) = linio([mdl, '/Micro-Station/Fn'],1,'input');
+io(2) = linio([mdl, '/Micro-Station/Nano_Hexapod'],1,'output');
% Run the linearization
G_f_to_d = linearize(mdl,io, 0);
+G_f_to_d = preprocessIdTf(G_f_to_d, 10, 10000);
+
% Input/Output names
-G_f_to_d.InputName = {'Fy'};
-G_f_to_d.OutputName = {'Dy'};
+G_f_to_d.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
+G_f_to_d.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
% Bode Plot of the linearized function
-figure;
-bode(G_f_to_d(2, 2), bode_opts)
+bodeFig({G_f_to_d(1, 1), G_f_to_d(2, 2), G_f_to_d(3, 3)}, struct('phase', true))
+legend({'$F_{n_x} \rightarrow D_{x}$', '$F_{n_y} \rightarrow D_{y}$', '$F_{n_z} \rightarrow D_{z}$'})
%%
-save('../mat/G_f_to_d.mat', 'G_f_to_d');
+save('./mat/G_f_to_d.mat', 'G_f_to_d');
diff --git a/Identification/identification_stages_run.m b/Identification/identification_stages_run.m
index 7c8b33a..c048e19 100644
--- a/Identification/identification_stages_run.m
+++ b/Identification/identification_stages_run.m
@@ -3,25 +3,11 @@
% Save all computed transfer functions into one .mat file
%%
-clear;
-close all;
-clc
-
-%% Define options for bode plots
-bode_opts = bodeoptions;
-
-bode_opts.Title.FontSize = 12;
-bode_opts.XLabel.FontSize = 12;
-bode_opts.YLabel.FontSize = 12;
-bode_opts.FreqUnits = 'Hz';
-bode_opts.MagUnits = 'abs';
-bode_opts.MagScale = 'log';
-bode_opts.PhaseWrapping = 'on';
-bode_opts.PhaseVisible = 'off';
+clear; close all; clc;
%% Options for preprocessing the identified transfer functions
-f_low = 10;
-f_high = 1000;
+f_low = 10; % [Hz]
+f_high = 10000; % [Hz]
%% Options for Linearized
options = linearizeOptions;
@@ -33,7 +19,7 @@ mdl = 'Assemblage';
%% Y-Translation Stage
% Input/Output definition
io(1) = linio([mdl, '/Fy'],1,'input');
-io(2) = linio([mdl, '/Translation y'],1,'output');
+io(2) = linio([mdl, '/Dy_meas'],1,'output');
% Run the linearization
G_ty_raw = linearize(mdl,io, 0);
@@ -46,13 +32,14 @@ G_ty.InputName = {'Fy'};
G_ty.OutputName = {'Dy'};
% Bode Plot of the linearized function
-figure;
-bode(G_ty, bode_opts)
+bodeFig({G_ty}, struct('phase', true))
+legend({'$F_{y} \rightarrow D_{y}$'})
+exportFig('id_ty', 'normal-normal')
%% Tilt Stage
% Input/Output definition
io(1) = linio([mdl, '/My'],1,'input');
-io(2) = linio([mdl, '/Tilt'],1,'output');
+io(2) = linio([mdl, '/Ry_meas'],1,'output');
% Run the linearization
G_ry_raw = linearize(mdl,io, 0);
@@ -65,14 +52,14 @@ G_ry.InputName = {'My'};
G_ry.OutputName = {'Ry'};
% Bode Plot of the linearized function
-figure;
-bode(G_ry, bode_opts)
-
+bodeFig({G_ry}, struct('phase', true))
+legend({'$M_{y} \rightarrow R_{y}$'})
+exportFig('id_ry', 'normal-normal')
%% Spindle
% Input/Output definition
io(1) = linio([mdl, '/Mz'],1,'input');
-io(2) = linio([mdl, '/Spindle'],1,'output');
+io(2) = linio([mdl, '/Rz_meas'],1,'output');
% Run the linearization
G_rz_raw = linearize(mdl,io, 0);
@@ -85,14 +72,14 @@ G_rz.InputName = {'Mz'};
G_rz.OutputName = {'Rz'};
% Bode Plot of the linearized function
-figure;
-bode(G_rz, bode_opts)
-
+bodeFig({G_rz}, struct('phase', true))
+legend({'$M_{z} \rightarrow R_{z}$'})
+exportFig('id_ry', 'normal-normal')
%% Hexapod Symetrie
% Input/Output definition
io(1) = linio([mdl, '/Fhexa_cart'],1,'input');
-io(2) = linio([mdl, '/Hexapod Symetrie'],1,'output');
+io(2) = linio([mdl, '/Dm_meas'],1,'output');
% Run the linearization
G_hexa_raw = linearize(mdl,io, 0);
@@ -102,19 +89,28 @@ G_hexa = preprocessIdTf(G_hexa_raw, f_low, f_high);
% Input/Output names
G_hexa.InputName = {'Fhexa_x', 'Fhexa_y', 'Fhexa_z', 'Mhexa_x', 'Mhexa_y', 'Mhexa_z'};
-G_hexa.OutputName = {'Dhexa_x', 'Dhexa_y', 'Dhexa_z', 'Dhexa_x', 'Dhexa_y', 'Dhexa_z'};
+G_hexa.OutputName = {'Dhexa_x', 'Dhexa_y', 'Dhexa_z', 'Rhexa_x', 'Rhexa_y', 'Rhexa_z'};
% Bode Plot of the linearized function
-figure;
-bode(G_hexa, bode_opts)
+bodeFig({G_hexa(1, 1), G_hexa(2, 2), G_hexa(3, 3)}, struct('phase', true))
+legend({'$F_{h_x} \rightarrow D_{h_x}$', '$F_{h_y} \rightarrow D_{h_y}$', '$F_{h_z} \rightarrow D_{h_z}$'})
+exportFig('id_hexapod_trans', 'normal-normal')
+
+bodeFig({G_hexa(4, 4), G_hexa(5, 5), G_hexa(6, 6)}, struct('phase', true))
+legend({'$M_{h_x} \rightarrow R_{h_x}$', '$M_{h_y} \rightarrow R_{h_y}$', '$M_{h_z} \rightarrow R_{h_z}$'})
+exportFig('id_hexapod_rot', 'normal-normal')
+
+bodeFig({G_hexa(1, 1), G_hexa(2, 1), G_hexa(3, 1)}, struct('phase', true))
+legend({'$F_{h_x} \rightarrow D_{h_x}$', '$F_{h_x} \rightarrow D_{h_y}$', '$F_{h_x} \rightarrow D_{h_z}$'})
+exportFig('id_hexapod_coupling', 'normal-normal')
%% NASS
% Input/Output definition
io(1) = linio([mdl, '/Fnass_cart'],1,'input');
-io(2) = linio([mdl, '/NASS'],1,'output');
+io(2) = linio([mdl, '/Dn_meas'],1,'output');
% Run the linearization
-G_nass_raw = linearize(mdl,io, 0);
+c = linearize(mdl,io, 0);
% Post-process the linearized function
G_nass = preprocessIdTf(G_nass_raw, f_low, f_high);
@@ -124,8 +120,17 @@ G_nass.InputName = {'Fnass_x', 'Fnass_y', 'Fnass_z', 'Mnass_x', 'Mnass_y', 'Mna
G_nass.OutputName = {'Dnass_x', 'Dnass_y', 'Dnass_z', 'Dnass_x', 'Dnass_y', 'Dnass_z'};
% Bode Plot of the linearized function
-figure;
-bode(G_nass, bode_opts)
+bodeFig({G_nass(1, 1), G_nass(2, 2), G_nass(3, 3)}, struct('phase', true))
+legend({'$F_{n_x} \rightarrow D_{n_x}$', '$F_{n_y} \rightarrow D_{n_y}$', '$F_{n_z} \rightarrow D_{n_z}$'})
+exportFig('id_nass_trans', 'normal-normal')
+
+bodeFig({G_nass(4, 4), G_nass(5, 5), G_nass(6, 6)}, struct('phase', true))
+legend({'$M_{n_x} \rightarrow R_{n_x}$', '$M_{n_y} \rightarrow R_{n_y}$', '$M_{n_z} \rightarrow R_{n_z}$'})
+exportFig('id_nass_rot', 'normal-normal')
+
+bodeFig({G_nass(1, 1), G_nass(2, 1), G_nass(3, 1)}, struct('phase', true))
+legend({'$F_{n_x} \rightarrow D_{n_x}$', '$F_{n_x} \rightarrow D_{n_y}$', '$F_{n_x} \rightarrow D_{n_z}$'})
+exportFig('id_nass_coupling', 'normal-normal')
%% Save all transfer function
save('../mat/identified_tf.mat', 'G_ty', 'G_ry', 'G_rz', 'G_hexa', 'G_nass')
diff --git a/Rotation_matrix_angles.m b/Rotation_matrix_angles.m
deleted file mode 100644
index 518669a..0000000
--- a/Rotation_matrix_angles.m
+++ /dev/null
@@ -1,3 +0,0 @@
-thetax=atan2(-R(2,3),R(3,3));
-thetay=atan2(R(1,3),sqrt((R(1,1))^2+(R(1,2))^2));
-thetaz=atan2(-R(1,2),R(1,1));
diff --git a/init_data.m b/init_data.m
index 8ec00dc..084e9e9 100644
--- a/init_data.m
+++ b/init_data.m
@@ -11,7 +11,7 @@ granite = struct();
granite.m = smiData.Solid(5).mass;
granite.k.ax = 1e8; % x-y-z Stiffness of the granite [N/m]
-granite.ksi.ax = 10;
+granite.ksi.ax = 1;
granite = updateDamping(granite);
@@ -23,8 +23,8 @@ ty.m = smiData.Solid(4).mass+smiData.Solid(6).mass+smiData.Solid(7).mass+smiData
ty.k.ax = 1e7/4; % Axial Stiffness for each of the 4 guidance (y) [N/m]
ty.k.rad = 9e9/4; % Radial Stiffness for each of the 4 guidance (x-z) [N/m]
-ty.ksi.ax = 10;
-ty.ksi.rad = 10;
+ty.ksi.ax = 0.05;
+ty.ksi.rad = 1;
ty = updateDamping(ty);
@@ -38,14 +38,13 @@ ry.k.rad = 555e6/4; % Stiffness in the top direction [N/m]
ry.k.rrad = 238e6/4; % Stiffness in the side direction [N/m]
ry.k.tilt = 1e4 ; % Rotation stiffness around y [N*m/deg]
-ry.ksi.h = 10;
-ry.ksi.rad = 10;
-ry.ksi.rrad = 10;
-ry.ksi.tilt = 10;
+ry.ksi.h = 1;
+ry.ksi.rad = 1;
+ry.ksi.rrad = 1;
+ry.ksi.tilt = 1;
ry = updateDamping(ry);
-
%% Spindle
rz = struct();
@@ -56,8 +55,8 @@ rz.k.rad = 7e8; % Radial Stiffness [N/m]
rz.k.tilt = 1e5; % TODO
rz.k.rot = 1e5; % Rotational Stiffness [N*m/deg]
-rz.ksi.ax = 10;
-rz.ksi.rad = 10;
+rz.ksi.ax = 1;
+rz.ksi.rad = 1;
rz.ksi.tilt = 1;
rz.ksi.rot = 1;
@@ -96,5 +95,6 @@ function element = updateDamping(element)
field = fieldnames(element.k);
for i = 1:length(field)
element.c.(field{i}) = 1/element.ksi.(field{i})*sqrt(element.k.(field{i})/element.m);
+% element.c.(field{i}) = element.k.(field{i})/1000;
end
end
diff --git a/init_inputs.m b/init_inputs.m
index a91636e..30e248e 100644
--- a/init_inputs.m
+++ b/init_inputs.m
@@ -8,7 +8,7 @@ time_vector = 0:Ts:Tsim;
%% Set point [m, rad]
setpoint = zeros(length(time_vector), 6);
-% setpoint(ceil(1/Ts):end, 2) = 1e-6;
+% setpoint(ceil(10/Ts):end, 2) = 1e-6; % Step of 1 micro-meter in y direction
r_setpoint = timeseries(setpoint, time_vector);
@@ -30,30 +30,38 @@ r_Gm = timeseries(xg, time_vector);
% plot(r_Gm)
%% Translation stage [m]
-r_Ty = timeseries(zeros(length(time_vector), 1), time_vector);
+ty = zeros(length(time_vector), 1);
+
+r_Ty = timeseries(ty, time_vector);
%% Tilt Stage [rad]
-% r_tilt = zeros(length(time_vector), 1);
+r_tilt = zeros(length(time_vector), 1);
-r_tilt = 3*2*pi/360*sin(2*pi*0.5*time_vector);
+% r_tilt = 3*(2*pi/360)*sin(2*pi*0.2*time_vector);
r_My = timeseries(r_tilt, time_vector);
%% Spindle [rad]
-% r_spindle = zeros(length(time_vector), 1);
+r_spindle = zeros(length(time_vector), 1);
-r_spindle = 2*pi*time_vector;
+% r_spindle = 2*pi*0.5*time_vector;
r_Mz = timeseries(r_spindle, time_vector);
%% Micro Hexapod
-r_u_hexa = timeseries(zeros(length(time_vector), 6), time_vector);
+u_hexa = zeros(length(time_vector), 6);
+
+r_u_hexa = timeseries(u_hexa, time_vector);
%% Center of gravity compensation
-r_mass = timeseries(zeros(length(time_vector), 2), time_vector);
+mass = zeros(length(time_vector), 2);
+
+r_mass = timeseries(mass, time_vector);
%% Nano Hexapod
-r_n_hexa = timeseries(zeros(length(time_vector), 6), time_vector);
+n_hexa = zeros(length(time_vector), 6);
+
+r_n_hexa = timeseries(n_hexa, time_vector);
%%
-save('./mat/inputs_setpoint.mat', 'r_setpoint', 'r_Gm', 'r_Ty', 'r_My', 'r_u_hexa', 'r_mass', 'r_n_hexa');
\ No newline at end of file
+save('./mat/inputs_setpoint.mat', 'r_setpoint', 'r_Gm', 'r_Ty', 'r_My', 'r_Mz', 'r_u_hexa', 'r_mass', 'r_n_hexa');
\ No newline at end of file
diff --git a/init_sim_configuration.m b/init_sim_configuration.m
index f7ed2ad..1e68c7f 100644
--- a/init_sim_configuration.m
+++ b/init_sim_configuration.m
@@ -1,6 +1,6 @@
%% Solver Configuration
Ts = 1e-4; % Sampling time [s]
-Tsim = 5; % Simulation time [s]
+Tsim = 10; % Simulation time [s]
%% Gravity
g = 0 ; % Gravity along the z axis [m/s^2]
diff --git a/mat/G_f_to_d.mat b/mat/G_f_to_d.mat
index e37ac05..48f4209 100644
Binary files a/mat/G_f_to_d.mat and b/mat/G_f_to_d.mat differ
diff --git a/mat/controller.mat b/mat/controller.mat
index 0dee083..e890e5e 100644
Binary files a/mat/controller.mat and b/mat/controller.mat differ
diff --git a/mat/inputs_setpoint.mat b/mat/inputs_setpoint.mat
index 64e9a46..5a3d02f 100644
Binary files a/mat/inputs_setpoint.mat and b/mat/inputs_setpoint.mat differ
diff --git a/src/preprocessIdTf.m b/src/preprocessIdTf.m
new file mode 100644
index 0000000..779b5a0
--- /dev/null
+++ b/src/preprocessIdTf.m
@@ -0,0 +1,5 @@
+function [G] = preprocessIdTf(G0, f_low, f_high)
+ [~,G1] = freqsep(G0, 2*pi*f_low);
+ [G2,~] = freqsep(G1, 2*pi*f_high);
+ G = minreal(G2);
+end
diff --git a/stewart-simscape b/stewart-simscape
index 17c9de5..6fe9603 160000
--- a/stewart-simscape
+++ b/stewart-simscape
@@ -1 +1 @@
-Subproject commit 17c9de54fb551cb5280283186ca58d4a7f48c39c
+Subproject commit 6fe96032fd93d81cfcb6c78fea8a79bb586dd488