[WIP] Add src folder, change damping of system

.SimulinkProject/Root.type.Files/Assemblage_DataFile.m.type.File.xml

@@ -1,6 +0,0 @@
-<?xml version='1.0' encoding='UTF-8'?>
-<Info>
-    <Category UUID="FileClassCategory">
-        <Label UUID="design" />
-    </Category>
-</Info>

.SimulinkProject/Root.type.ProjectPath/2793b451-85f9-44a8-8d10-25d7657da4e0.type.Reference.xml

@@ -0,0 +1,2 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<Info Ref="Identification" Type="Relative" />

.SimulinkProject/Root.type.ProjectPath/74f6c052-8691-4256-bd9d-7bbb44c7ca82.type.Reference.xml

@@ -0,0 +1,2 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<Info Ref="src" Type="Relative" />

.SimulinkProject/Root.type.ProjectPath/c767e49c-8d99-42c0-ab3b-d872f82e48d2.type.Reference.xml

@@ -0,0 +1,2 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<Info Ref="Analysis" Type="Relative" />

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;
+bodeFig({gain*G}, struct('phase', true))
-bode(gain*G, bode_opts)
 
 %%
 [~,~,~,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;
+bodeFig({C*G}, struct('phase', true))
-bode(Ky*G, bode_opts)
 
 %%
 K = tf(zeros(6));
-K(2,2) = Ky;
+K(2,2) = C;
 
 %%
-save('../mat/controller.mat', 'K')
+save('./mat/controller.mat', 'K')

Identification/identification_control.m

@@ -1,25 +1,11 @@
 %% Script Description
 % 
 %%
-clear;
+clear; close all; clc;
-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';
 
 %% 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(1) = linio([mdl, '/Micro-Station/Fn'],1,'input');
-io(2) = linio([mdl, '/Sample'],1,'output');
+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.InputName  = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
-G_f_to_d.OutputName = {'Dy'};
+G_f_to_d.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
 
 % Bode Plot of the linearized function
-figure;
+bodeFig({G_f_to_d(1, 1), G_f_to_d(2, 2), G_f_to_d(3, 3)}, struct('phase', true))
-bode(G_f_to_d(2, 2), bode_opts)
+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');

Identification/identification_stages_run.m

@@ -3,25 +3,11 @@
 % Save all computed transfer functions into one .mat file
 
 %%
-clear;
+clear; close all; clc;
-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';
 
 %% Options for preprocessing the identified transfer functions
-f_low = 10;
+f_low  = 10;    % [Hz]
-f_high = 1000;
+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;
+bodeFig({G_ty}, struct('phase', true))
-bode(G_ty, bode_opts)
+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;
+bodeFig({G_ry}, struct('phase', true))
-bode(G_ry, bode_opts)
+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;
+bodeFig({G_rz}, struct('phase', true))
-bode(G_rz, bode_opts)
+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;
+bodeFig({G_hexa(1, 1), G_hexa(2, 2), G_hexa(3, 3)}, struct('phase', true))
-bode(G_hexa, bode_opts)
+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;
+bodeFig({G_nass(1, 1), G_nass(2, 2), G_nass(3, 3)}, struct('phase', true))
-bode(G_nass, bode_opts)
+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')

Rotation_matrix_angles.m

@@ -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));

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.ax   = 0.05;
-ty.ksi.rad  = 10;
+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.h    = 1;
-ry.ksi.rad  = 10;
+ry.ksi.rad  = 1;
-ry.ksi.rrad = 10;
+ry.ksi.rrad = 1;
-ry.ksi.tilt = 10;
+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.ax   = 1;
-rz.ksi.rad  = 10;
+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

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');
+save('./mat/inputs_setpoint.mat', 'r_setpoint', 'r_Gm', 'r_Ty', 'r_My', 'r_Mz', 'r_u_hexa', 'r_mass', 'r_n_hexa');

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]

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

stewart-simscape

@@ -1 +1 @@
-Subproject commit 17c9de54fb551cb5280283186ca58d4a7f48c39c
+Subproject commit 6fe96032fd93d81cfcb6c78fea8a79bb586dd488