tdehaeze/nass-simscape
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Huge Change

Browse Source 
- Add may folders
- Add IFF and HAC-LAC scripts
This commit is contained in:
Thomas Dehaeze 2018-10-07 22:07:21 +02:00
parent 496dd15586
commit f0e83d7c39
84 changed files with 1292 additions and 425 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.SimulinkProject/Root.type.ProjectPath/1809d361-dc3d-4fcb-8b9b-833180e2f6fc.type.Reference.xml Normal file
View File

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

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

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

2
.SimulinkProject/Root.type.ProjectPath/2d80ad84-a2e6-469a-be37-c276ae1af074.type.Reference.xml → .SimulinkProject/Root.type.ProjectPath/56b46427-db89-446e-ab1b-3a8188f5e1b3.type.Reference.xml
View File

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

2
.SimulinkProject/Root.type.ProjectPath/c767e49c-8d99-42c0-ab3b-d872f82e48d2.type.Reference.xml → .SimulinkProject/Root.type.ProjectPath/7e53593c-166d-46eb-8350-609f08763240.type.Reference.xml
View File

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

2
.SimulinkProject/Root.type.ProjectPath/83f0e724-9931-453a-b699-3bd293db000c.type.Reference.xml Normal file
View File

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

2
.SimulinkProject/Root.type.ProjectPath/a1959b94-b957-42df-87b8-4945482358a6.type.Reference.xml Normal file
View File

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

2
.SimulinkProject/Root.type.ProjectPath/d03bc20c-98e3-4ed4-a859-51e1f5a18637.type.Reference.xml Normal file
View File

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

BIN
Assemblage.slx
View File

Binary file not shown.

23
Control/cl_tf.m
View File

@ -1,23 +0,0 @@
%%
clear; close all; clc;
%% Load Plant
load('./mat/G_xg_to_d.mat', 'G_xg_to_d');
load('./mat/G_f_to_d.mat', 'G_1', 'G_20', 'G_50');
load('./mat/controller.mat', 'K');
%%
S = minreal(inv(tf(eye(6))+G_20*K));
T = minreal((tf(eye(6))+G_20*K)\G_20*K);
bodeFig({S(1,1), T(1,1)})
legend({'$S_x$', '$T_x$'})
bodeFig({S(2,2), T(2,2)})
legend({'$S_y$', '$T_y$'})
bodeFig({S(3,3), T(3,3)})
legend({'$S_z$', '$T_z$'})
%%
save('./mat/T_S.mat', 'S', 'T');

55
Control/generate_control.m
View File

@ -1,55 +0,0 @@
%%
clear; close all; clc;
%% Load Plant
load('./mat/G_f_to_d.mat', 'G_20');
%% Load previously generated controllers
load('./mat/control_K_tx.mat', 'K_tx');
load('./mat/control_K_ty.mat', 'K_ty');
load('./mat/control_K_tz.mat', 'K_tz');
load('./mat/control_K_rx.mat', 'K_rx');
load('./mat/control_K_ry.mat', 'K_ry');
load('./mat/control_K_rz.mat', 'K_rz');
%%
sisotool('bode', G_20(1, 1), K_tx);
K_tx = C;
save('./mat/control_K_tx.mat', 'K_tx');
%%
sisotool('bode', G_20(2, 2), K_ty);
K_ty = C;
save('./mat/control_K_ty.mat', 'K_ty');
%%
sisotool('bode', G_20(3, 3), K_tz);
K_tz = C;
save('./mat/control_K_tz.mat', 'K_tz');
%%
sisotool('bode', G_20(4, 4), K_rx);
K_rx = C;
save('./mat/control_K_rx.mat', 'K_rx');
%%
sisotool('bode', G_20(5, 5), K_ry);
K_ry = C;
save('./mat/control_K_ry.mat', 'K_ry');
%%
sisotool('bode', G_20(6, 6), K_rz);
K_rz = C;
save('./mat/control_K_rz.mat', 'K_rz');
%%
K = tf(zeros(6));
K(1,1) = K_tx;
K(2,2) = K_ty;
K(3,3) = K_tz;
K(4,4) = K_rx;
K(5,5) = K_ry;
K(6,6) = K_rz;
%% Save the MIMO control
save('./mat/controller.mat', 'K');

BIN
Micro_Station_Displacement.slx
View File

Binary file not shown.

28
active_damping/act_damp_main.m Normal file
View File

@ -0,0 +1,28 @@
%%
clear; close all; clc;
%% IFF: Integral Force Feedback Control
% Generate the IFF Control Laws
run iff_control.m
% Identification of the TF of damped system
run iff_identification.m
% Compare undamped and damped system
run iff_comp_tf.m
% Generate Control Laws with the damped system
run iff_fb_control.m
% Plot Loop Gains for the new control laws
run iff_fb_control_plots.m
% Simulation of the damped system
run iff_simulation.m
% Plot results of the simulations
run iff_results.m
%% DVF: Direct Velocity Feedback
% Generate the DVF Control Laws
run dvf_control.m

22
active_damping/dvf_control.m Normal file
View File

@ -0,0 +1,22 @@
%%
clear; close all; clc;
%% Load the identified transfer functions
load('./mat/G.mat', 'G_light_vc', 'G_light_pz', 'G_heavy_vc', 'G_heavy_pz');
%% Load Configuration file
load('./mat/config.mat', 'save_fig', 'freqs');
%%
s = tf('s');
%%
% sisotool(-G_heavy_pz.G_dvf('Vnx', 'Fnx'))
K_dvf_light_vc = tf(eye(6));
K_dvf_light_pz = tf(eye(6));
K_dvf_heavy_vc = tf(eye(6));
K_dvf_heavy_pz = tf(eye(6));
%%
save('./mat/K_dvf_crit.mat', 'K_dvf_light_vc', 'K_dvf_light_pz', 'K_dvf_heavy_vc', 'K_dvf_heavy_pz');

74
active_damping/iff_comp_tf.m Normal file
View File

@ -0,0 +1,74 @@
%%
clear; close all; clc;
%% Load System and Damped System
load('./mat/G.mat', 'G_light_vc', 'G_light_pz', 'G_heavy_vc', 'G_heavy_pz');
load('./mat/G_iff.mat', 'G_iff_light_vc', 'G_iff_light_pz', 'G_iff_heavy_vc', 'G_iff_heavy_pz');
%% Load Configuration file
load('./mat/config.mat', 'save_fig', 'freqs');
%% New Plant damped
figure;
% Amplitude
ax1 = subaxis(2,1,1);
hold on;
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '-', 'DisplayName', 'Light VC');
plot(freqs, abs(squeeze(freqresp(G_light_pz.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '-', 'DisplayName', 'Light PZ');
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_iff_light_vc.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--', 'DisplayName', 'Damped');
plot(freqs, abs(squeeze(freqresp(G_iff_light_pz.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--', 'DisplayName', 'Damped');
set(gca,'xscale','log'); set(gca,'yscale','log');
ylabel('Amplitude [m/N]');
set(gca, 'XTickLabel',[]);
legend('Location', 'southwest');
hold off;
% Phase
ax2 = subaxis(2,1,2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_vc.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '-');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_pz.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '-');
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff_light_vc.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff_light_pz.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--');
set(gca,'xscale','log');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
linkaxes([ax1,ax2],'x');
xlim([freqs(1) freqs(end)]);
if save_fig; exportFig('damping_comp_plant', 'normal-normal', struct('path', 'active_damping')); end
%% From xw to d
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), '-', 'DisplayName', 'Light VC');
plot(freqs, abs(squeeze(freqresp(G_light_pz.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), '-', 'DisplayName', 'Light PZ');
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_iff_light_vc.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), '--', 'DisplayName', 'Damped');
plot(freqs, abs(squeeze(freqresp(G_iff_light_pz.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), '--', 'DisplayName', 'Damped');
hold off;
xlim([freqs(1) freqs(end)]);
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
legend('Location', 'southwest');
if save_fig; exportFig('damping_comp_xw', 'normal-normal', struct('path', 'active_damping')); end
%% From fi to d
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), '-', 'DisplayName', 'Light VC');
plot(freqs, abs(squeeze(freqresp(G_light_pz.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), '-', 'DisplayName', 'Light PZ');
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_iff_light_vc.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), '--', 'DisplayName', 'Damped');
plot(freqs, abs(squeeze(freqresp(G_iff_light_pz.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), '--', 'DisplayName', 'Damped');
hold off;
xlim([freqs(1) freqs(end)]);
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
legend('Location', 'southwest');
if save_fig; exportFig('damping_comp_fi', 'normal-normal', struct('path', 'active_damping')); end

22
active_damping/iff_control.m Normal file
View File

@ -0,0 +1,22 @@
%%
clear; close all; clc;
%% Load the identified transfer functions
load('./mat/G.mat', 'G_light_vc', 'G_light_pz', 'G_heavy_vc', 'G_heavy_pz');
%% Load Configuration file
load('./mat/config.mat', 'save_fig', 'freqs');
%%
s = tf('s');
%%
% sisotool(-G_heavy_pz.G_iff('Fm1', 'F1')/s)
K_iff_light_vc = 48/s*tf(eye(6));
K_iff_light_pz = 1500/s*tf(eye(6));
K_iff_heavy_vc = 20/s*tf(eye(6));
K_iff_heavy_pz = 535/s*tf(eye(6));
%%
save('./mat/K_iff_crit.mat', 'K_iff_light_vc', 'K_iff_light_pz', 'K_iff_heavy_vc', 'K_iff_heavy_pz');

34
active_damping/iff_identification.m Normal file
View File

@ -0,0 +1,34 @@
%%
clear; close all; clc;
%% Load IFF Controllers
load('./mat/K_iff_crit.mat', 'K_iff_light_vc', 'K_iff_light_pz', 'K_iff_heavy_vc', 'K_iff_heavy_pz');
%% Light Sample
initializeSample(struct('mass', 1));
initializeNanoHexapod(struct('actuator', 'lorentz'));
K_iff = K_iff_light_vc; %#ok
save('./mat/K_iff.mat', 'K_iff');
G_iff_light_vc = identifyPlant();
initializeNanoHexapod(struct('actuator', 'piezo'));
K_iff = K_iff_light_pz; %#ok
save('./mat/K_iff.mat', 'K_iff');
G_iff_light_pz = identifyPlant();
%% Heavy Sample
initializeSample(struct('mass', 50));
initializeNanoHexapod(struct('actuator', 'lorentz'));
K_iff = K_iff_heavy_vc; %#ok
save('./mat/K_iff.mat', 'K_iff');
G_iff_heavy_vc = identifyPlant();
initializeNanoHexapod(struct('actuator', 'piezo'));
K_iff = K_iff_heavy_pz;
save('./mat/K_iff.mat', 'K_iff');
G_iff_heavy_pz = identifyPlant();
%% Save the obtained transfer functions
save('./mat/G_iff.mat', 'G_iff_light_vc', 'G_iff_light_pz', 'G_iff_heavy_vc', 'G_iff_heavy_pz');

39
active_damping/iff_results.m Normal file
View File

@ -0,0 +1,39 @@
%%
clear; close all; clc;
%% Load Configuration file
load('./mat/config.mat', 'save_fig', 'freqs');
%% Load Simulation Results
sim_light_vc_ol = load('./mat/sim_light_vc_ol_none.mat', 'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
sim_light_pz_ol = load('./mat/sim_light_pz_ol_none.mat', 'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
sim_light_vc_cl = load('./mat/sim_light_vc_cl_none.mat', 'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
sim_light_pz_cl = load('./mat/sim_light_pz_cl_none.mat', 'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
sim_light_vc_ol_iff = load('./mat/sim_light_vc_ol_iff.mat', 'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
sim_light_pz_ol_iff = load('./mat/sim_light_pz_ol_iff.mat', 'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
sim_light_vc_cl_iff = load('./mat/sim_light_vc_cl_iff.mat', 'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
sim_light_pz_cl_iff = load('./mat/sim_light_pz_cl_iff.mat', 'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
%%
figure;
hold on;
plot(sim_light_vc_ol.Dx, sim_light_vc_ol.Dy);
plot(sim_light_vc_cl.Dx, sim_light_vc_cl.Dy);
plot(sim_light_vc_ol_iff.Dx, sim_light_vc_ol_iff.Dy);
plot(sim_light_vc_cl_iff.Dx, sim_light_vc_cl_iff.Dy);
hold off;
%%
rms(sqrt(sim_light_vc_ol.Dx.^2+sim_light_vc_ol.Dy.^2))
rms(sqrt(sim_light_vc_cl.Dx.^2+sim_light_vc_cl.Dy.^2))
rms(sqrt(sim_light_vc_ol_iff.Dx.^2+sim_light_vc_ol_iff.Dy.^2))
rms(sqrt(sim_light_vc_cl_iff.Dx.^2+sim_light_vc_cl_iff.Dy.^2))
%%
rms(sqrt(sim_light_pz_ol.Dx.^2+sim_light_pz_ol.Dy.^2))
rms(sqrt(sim_light_pz_cl.Dx.^2+sim_light_pz_cl.Dy.^2))
rms(sqrt(sim_light_pz_ol_iff.Dx.^2+sim_light_pz_ol_iff.Dy.^2))
rms(sqrt(sim_light_pz_cl_iff.Dx.^2+sim_light_pz_cl_iff.Dy.^2))

11
active_damping/iff_simulation.m Normal file
View File

@ -0,0 +1,11 @@
%%
clear; close all; clc;
%% Initialize Simulation and Inputs
initializeExperiment('tomography', 'light');
%% Run Open Loop Simulations
runSimulation('vc', 'light', 'ol', 'iff');
runSimulation('pz', 'light', 'ol', 'iff');
% runSimulation('vc', 'heavy', 'ol', 'iff');
% runSimulation('pz', 'heavy', 'ol', 'iff');

7
config.m
View File

@ -1,10 +1,11 @@
%% Add folders to Matlab Path
% addpath('./Analysis/');
% addpath('./Control/');
% addpath('./Identification/');
% addpath('./analysis/');
% addpath('./control/');
% addpath('./identification/');
% addpath('./initialize/');
% addpath('./src/');
% addpath('./stewart-simscape/');
% addpath('./active_damping/');
%%
freqs = logspace(-1, 3, 1000);

90
control/control_cl_ol_plots.m Normal file
View File

@ -0,0 +1,90 @@
%%
clear; close all; clc;
%% Load Configuration file
load('./mat/config.mat', 'save_fig', 'freqs');
%% Load Simulation Results
sim_light_vc_ol = load('./mat/sim_light_vc_ol.mat', 'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
sim_light_vc_cl = load('./mat/sim_light_vc_cl.mat', 'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
sim_light_pz_ol = load('./mat/sim_light_pz_ol.mat', 'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
sim_light_pz_cl = load('./mat/sim_light_pz_cl.mat', 'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
%% Start after few seconds
T_init = 1;
%% Plot X against Y in OL - Piezo and Voice Coil
figure;
hold on;
plot(1e9*sim_light_vc_ol.Dx, 1e9*sim_light_vc_ol.Dy);
plot(1e9*sim_light_pz_ol.Dx, 1e9*sim_light_pz_ol.Dy);
hold off;
xlabel('X Displacement [nm]'); ylabel('Y Displacement [nm]');
xlim([-1000 1000]); ylim([-1000 1000]);
xticks(-1000:200:1000); yticks(-1000:200:1000);
legend({'VC - Light - OL', 'PZ - Light - OL'});
if save_fig; exportFig('xy_ol_vc_pz', 'normal-normal', struct('path', 'control')); end
%% Plot X against Y in CL - Piezo and Voice Coil
figure;
hold on;
plot(1e9*sim_light_vc_cl.Dx, 1e9*sim_light_vc_cl.Dy);
plot(1e9*sim_light_pz_cl.Dx, 1e9*sim_light_pz_cl.Dy);
hold off;
xlabel('X Displacement [nm]'); ylabel('Y Displacement [nm]');
xlim([-500 500]); ylim([-500 500]);
xticks(-500:100:500); yticks(-500:100:500);
legend({'VC - Light - CL', 'PZ - Light - CL'});
if save_fig; exportFig('xy_cl_vc_pz', 'normal-normal', struct('path', 'control')); end
%% Compute the RMS Values
i_init = find(sim_light_vc_ol.time > T_init, 1);
rms_light_vc_ol = rms(sqrt(sim_light_vc_ol.Dx(i_init:end).^2+sim_light_vc_ol.Dy(i_init:end).^2));
rms_light_pz_ol = rms(sqrt(sim_light_pz_ol.Dx(i_init:end).^2+sim_light_pz_ol.Dy(i_init:end).^2));
rms_light_vc_cl = rms(sqrt(sim_light_vc_cl.Dx(i_init:end).^2+sim_light_vc_cl.Dy(i_init:end).^2));
rms_light_pz_cl = rms(sqrt(sim_light_pz_cl.Dx(i_init:end).^2+sim_light_pz_cl.Dy(i_init:end).^2));
fprintf(' \t OL \t CL [nm RMS]\n');
fprintf('PZ \t %.0f \t %.0f \n', 1e9*rms_light_pz_ol, 1e9*rms_light_pz_cl);
fprintf('VC \t %.0f \t %.0f \n\n', 1e9*rms_light_vc_ol, 1e9*rms_light_vc_cl);
%% Compute the PSD
sim_light_vc_ol.psd = computePsdDispl(sim_light_vc_ol, 1, 2);
sim_light_pz_ol.psd = computePsdDispl(sim_light_pz_ol, 1, 2);
sim_light_vc_cl.psd = computePsdDispl(sim_light_vc_cl, 1, 2);
sim_light_pz_cl.psd = computePsdDispl(sim_light_pz_cl, 1, 2);
%% PSD Open Loop and Close Loop for the X direction
figure;
hold on;
plot(sim_light_vc_ol.psd.f, sim_light_vc_ol.psd.dx, 'DisplayName', 'VC - $T_x$ - OL');
plot(sim_light_pz_ol.psd.f, sim_light_pz_ol.psd.dx, 'DisplayName', 'PZ - $T_x$ - OL');
set(gca,'ColorOrderIndex',1);
plot(sim_light_vc_cl.psd.f, sim_light_vc_cl.psd.dx, '--', 'DisplayName', 'VC - $T_x$ - CL');
plot(sim_light_pz_cl.psd.f, sim_light_pz_cl.psd.dx, '--', 'DisplayName', 'PZ - $T_x$ - CL');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [$m^2/Hz$]'); xlabel('Frequency [Hz]');
xlim([sim_light_vc_ol.psd.f(1), sim_light_vc_ol.psd.f(end)])
hold off;
legend('Location', 'southwest');
if save_fig; exportFig('psd_ol_cl_pz_vc_light_tx', 'normal-normal', struct('path', 'control')); end
%% PSD Open Loop and Close Loop for the Z direction
figure;
hold on;
plot(sim_light_vc_ol.psd.f, sim_light_vc_ol.psd.dz, 'DisplayName', 'VC - $T_z$ - OL');
plot(sim_light_pz_ol.psd.f, sim_light_pz_ol.psd.dz, 'DisplayName', 'PZ - $T_z$ - OL');
set(gca,'ColorOrderIndex',1);
plot(sim_light_vc_cl.psd.f, sim_light_vc_cl.psd.dz, '--', 'DisplayName', 'VC - $T_z$ - CL');
plot(sim_light_pz_cl.psd.f, sim_light_pz_cl.psd.dz, '--', 'DisplayName', 'PZ - $T_z$ - CL');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [$m^2/Hz$]'); xlabel('Frequency [Hz]');
xlim([sim_light_vc_ol.psd.f(1), sim_light_vc_ol.psd.f(end)])
hold off;
legend('Location', 'southwest');
if save_fig; exportFig('psd_ol_cl_pz_vc_light_tz', 'normal-normal', struct('path', 'control')); end

11
control/control_cl_sim.m Normal file
View File

@ -0,0 +1,11 @@
%%
clear; close all; clc;
%% Initialize Simulation and Inputs
initializeExperiment('tomography', 'light');
%% Run Close Loop Simulations
runSimulation('vc', 'light', 'cl', 'none');
runSimulation('pz', 'light', 'cl', 'none');
% runSimulation('vc', 'heavy', 'cl', 'none');
% runSimulation('pz', 'heavy', 'cl', 'none');

34
control/control_cl_tf.m Normal file
View File

@ -0,0 +1,34 @@
%%
clear; close all; clc;
%% Load Controllers
load('./mat/K_fb.mat', 'K_light_vc', 'K_light_pz', 'K_heavy_vc', 'K_heavy_pz');
%% Closed Loop - Light Sample
initializeSample(struct('mass', 1));
initializeNanoHexapod(struct('actuator', 'lorentz'));
K = K_light_vc; %#ok
save('./mat/controller.mat', 'K');
Gd_cl_light_vc = identifyPlant();
initializeNanoHexapod(struct('actuator', 'piezo'));
K = K_light_pz; %#ok
save('./mat/controller.mat', 'K');
Gd_cl_light_pz = identifyPlant();
%% Closed Loop - Heavy Sample
initializeSample(struct('mass', 50));
initializeNanoHexapod(struct('actuator', 'lorentz'));
K = K_heavy_vc; %#ok
save('./mat/controller.mat', 'K');
G_cl_heavy_vc = identifyPlant();
initializeNanoHexapod(struct('actuator', 'piezo'));
K = K_heavy_pz;
save('./mat/controller.mat', 'K');
G_cl_heavy_pz = identifyPlant();
%% Save the identified transfer functions
save('./mat/G_cl.mat', 'G_cl_light_vc', 'G_cl_light_pz', 'G_cl_heavy_vc', 'G_cl_heavy_pz');

41
control/control_cl_tf_comp.m Normal file
View File

@ -0,0 +1,41 @@
%%
clear; close all; clc;
%% Load System and Damped System
load('./mat/G.mat', 'G_light_vc', 'G_light_pz', 'G_heavy_vc', 'G_heavy_pz');
load('./mat/G_cl.mat', 'G_cl_light_vc', 'G_cl_light_pz', 'G_cl_heavy_vc', 'G_cl_heavy_pz');
%% Load Configuration file
load('./mat/config.mat', 'save_fig', 'freqs');
%% From xw to d
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), '-', 'DisplayName', 'Light VC - OL');
plot(freqs, abs(squeeze(freqresp(G_light_pz.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), '-', 'DisplayName', 'Light PZ - OL');
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_cl_light_vc.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), '--', 'DisplayName', 'Light VC - CL');
plot(freqs, abs(squeeze(freqresp(G_cl_light_pz.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), '--', 'DisplayName', 'Light PZ - CL');
hold off;
xlim([freqs(1) freqs(end)]);
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
legend('Location', 'southwest');
if save_fig; exportFig('damping_comp_xw', 'normal-normal', struct('path', 'active_damping')); end
%% From fi to d
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), '-', 'DisplayName', 'Light VC - OL');
plot(freqs, abs(squeeze(freqresp(G_light_pz.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), '-', 'DisplayName', 'Light PZ - OL');
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_cl_light_vc.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), '--', 'DisplayName', 'Light VC - CL');
plot(freqs, abs(squeeze(freqresp(G_cl_light_pz.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), '--', 'DisplayName', 'Light PZ - CL');
hold off;
xlim([freqs(1) freqs(end)]);
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
legend('Location', 'southwest');
if save_fig; exportFig('damping_comp_fi', 'normal-normal', struct('path', 'active_damping')); end

17
control/control_generate.m Normal file
View File

@ -0,0 +1,17 @@
%%
clear; close all; clc;
%% Load Plant
load('./mat/G.mat', 'G_light_vc', 'G_light_pz', 'G_heavy_vc', 'G_heavy_pz');
%%
fs = 10;
K_light_vc = generateDiagPidControl(G_light_vc.G_cart, fs);
K_light_pz = generateDiagPidControl(G_light_pz.G_cart, fs);
K_heavy_vc = generateDiagPidControl(G_heavy_vc.G_cart, fs);
K_heavy_pz = generateDiagPidControl(G_heavy_pz.G_cart, fs);
%% Save the MIMO control
save('./mat/K_fb.mat', 'K_light_vc', 'K_light_pz', 'K_heavy_vc', 'K_heavy_pz');

25
control/control_main.m Normal file
View File

@ -0,0 +1,25 @@
%%
clear; close all; clc;
%% Generate Control Laws for the Undamped System
run control_generate.m
%% Run the simulation and save results
% Run open loop simulations
run control_ol_sim.m
% Run closed loop simulations
run control_cl_sim.m
% Compute PSD in open loop
run control_ol_psd.m
% Plots to compare OL and CL for PZ and VC
run control_cl_ol_plots.m
%% Identify the Closed Loop Transfer Functions
% Compute the closed loop transfer functions
run control_cl_tf.m
% Compare OL and CL transfer functions
run control_cl_tf_comp.m

87
control/control_ol_psd.m Normal file
View File

@ -0,0 +1,87 @@
%%
clear; close all; clc;
%% Load Configuration file
load('./mat/config.mat', 'save_fig', 'freqs');
%% Load Simulation Results
sim_light_vc_ol = load('./mat/sim_light_vc_ol.mat', 'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
sim_light_pz_ol = load('./mat/sim_light_pz_ol.mat', 'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
sim_heavy_vc_ol = load('./mat/sim_heavy_vc_ol.mat', 'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
sim_heavy_pz_ol = load('./mat/sim_heavy_pz_ol.mat', 'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
%%
sim_light_vc_ol.psd = computePsdDispl(sim_light_vc_ol, 1, 2);
sim_light_pz_ol.psd = computePsdDispl(sim_light_pz_ol, 1, 2);
sim_heavy_vc_ol.psd = computePsdDispl(sim_heavy_vc_ol, 1, 2);
sim_heavy_pz_ol.psd = computePsdDispl(sim_heavy_pz_ol, 1, 2);
%% PSD Plot of translations
figure;
hold on;
plot(sim_light_vc_ol.psd.f, sim_light_vc_ol.psd.dx);
plot(sim_light_vc_ol.psd.f, sim_light_vc_ol.psd.dy);
plot(sim_light_vc_ol.psd.f, sim_light_vc_ol.psd.dz);
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [$m^2/Hz$]'); xlabel('Frequency [Hz]');
hold off;
legend({'PSD $Tx$', 'PSD $Tz$', 'PSD $Tz$'})
if save_fig; exportFig('psd_ol_vc_light_trans', 'normal-normal', struct('path', 'control')); end
%% PSD Plot of rotations
figure;
hold on;
plot(sim_light_vc_ol.psd.f, sim_light_vc_ol.psd.rx);
plot(sim_light_vc_ol.psd.f, sim_light_vc_ol.psd.ry);
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [$rad^2/Hz$]'); xlabel('Frequency [Hz]');
hold off;
legend({'PSD $Rx$', 'PSD $Rz$'})
if save_fig; exportFig('psd_ol_vc_light_rot', 'normal-normal', struct('path', 'control')); end
%% PSD Plot of translations
figure;
hold on;
plot(sim_light_pz_ol.psd.f, sim_light_pz_ol.psd.dx);
plot(sim_light_pz_ol.psd.f, sim_light_pz_ol.psd.dy);
plot(sim_light_pz_ol.psd.f, sim_light_pz_ol.psd.dz);
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [$m^2/Hz$]'); xlabel('Frequency [Hz]');
hold off;
legend({'PSD $Tx$', 'PSD $Tz$', 'PSD $Tz$'})
if save_fig; exportFig('psd_ol_pz_light_trans', 'normal-normal', struct('path', 'control')); end
%% PSD Plot of rotations
figure;
hold on;
plot(sim_light_pz_ol.psd.f, sim_light_pz_ol.psd.rx);
plot(sim_light_pz_ol.psd.f, sim_light_pz_ol.psd.ry);
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [$rad^2/Hz$]'); xlabel('Frequency [Hz]');
hold off;
legend({'PSD $Rx$', 'PSD $Rz$'})
if save_fig; exportFig('psd_ol_pz_light_rot', 'normal-normal', struct('path', 'control')); end
%% PSD Plot of translations
figure;
hold on;
plot(sim_light_vc_ol.psd.f, sim_light_vc_ol.psd.dx);
plot(sim_light_vc_ol.psd.f, sim_light_vc_ol.psd.dy);
plot(sim_light_vc_ol.psd.f, sim_light_vc_ol.psd.dz);
set(gca,'ColorOrderIndex',1);
plot(sim_light_pz_ol.psd.f, sim_light_pz_ol.psd.dx, '--');
plot(sim_light_pz_ol.psd.f, sim_light_pz_ol.psd.dy, '--');
plot(sim_light_pz_ol.psd.f, sim_light_pz_ol.psd.dz, '--');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [$m^2/Hz$]'); xlabel('Frequency [Hz]');
xlim([sim_light_vc_ol.psd.f(1), sim_light_vc_ol.psd.f(end)])
hold off;
legend({'PSD $Tx$ - VC', 'PSD $Tz$ - VC', 'PSD $Tz$ - VC', 'PSD $Tx$ - PZ', 'PSD $Tz$ - PZ', 'PSD $Tz$ - PZ'})
if save_fig; exportFig('psd_ol_pz_vc_light_trans', 'wide-tall', struct('path', 'control')); end

11
control/control_ol_sim.m Normal file
View File

@ -0,0 +1,11 @@
%%
clear; close all; clc;
%% Initialize Simulation and Inputs
initializeExperiment('tomography', 'light');
%% Run Open Loop Simulations
runSimulation('vc', 'light', 'ol', 'none');
runSimulation('pz', 'light', 'ol', 'none');
% runSimulation('vc', 'heavy', 'ol', 'none');
% runSimulation('pz', 'heavy', 'ol', 'none');

BIN
demonstration/Micro_Station_Displacement.slx Normal file
View File

Binary file not shown.

14
demonstration/demonstration_main.m Normal file
View File

@ -0,0 +1,14 @@
%%
clear; close all; clc;
%% Demonstration of stroke of each stage
% Initalize data for demonstration
run displacement_init.m
% Run the simulation
run displacement_sim.m
%% Test the measurement of sample position
run sample_pos_init.m
run sample_pos_sim.m

0
init_data_demonstration.m → demonstration/displacement_init.m
View File

5
demonstration/displacement_sim.m Normal file
View File

@ -0,0 +1,5 @@
%%
clear; close all; clc;
%%
sim('Micro_Station_Displacement.slx');

69
demonstration/sample_pos_init.m Normal file
View File

@ -0,0 +1,69 @@
%%
clear; close all; clc;
%% Initialize simulation configuration
opts_sim = struct(...
'Tsim', 2 ...
);
initializeSimConf(opts_sim);
%% Initialize Inputs
load('./mat/sim_conf.mat', 'sim_conf')
time_vector = 0:sim_conf.Ts:sim_conf.Tsim;
% Translation Stage
ty = 0*ones(length(time_vector), 1);
% Tilt Stage
ry = 2*pi*(3/360)*ones(length(time_vector), 1);
% Spindle
rz = 2*pi*1*(time_vector);
% Micro Hexapod
u_hexa = zeros(length(time_vector), 6);
% Gravity Compensator system
mass = zeros(length(time_vector), 2);
opts_inputs = struct(...
'ty', ty, ...
'ry', ry, ...
'rz', rz, ...
'u_hexa', u_hexa, ...
'mass', mass ...
);
initializeInputs(opts_inputs);
%% Initialize SolidWorks Data
initializeSmiData();
%% Initialize Ground
initializeGround();
%% Initialize Granite
initializeGranite();
%% Initialize Translation stage
initializeTy();
%% Initialize Tilt Stage
initializeRy();
%% Initialize Spindle
initializeRz();
%% Initialize Hexapod Symétrie
initializeMicroHexapod();
%% Initialize Center of Gravity compensation
initializeAxisc();
%% Initialize NASS
initializeNanoHexapod(struct('actuator', 'piezo'));
%% Initialize Sample
initializeSample(struct('mass', 20));

5
demonstration/sample_pos_sim.m Normal file
View File

@ -0,0 +1,5 @@
%%
clear; close all; clc;
%%
sim('Micro_Station_Displacement.slx');

17
hac_lac/hac_lac_iff_control.m Normal file
View File

@ -0,0 +1,17 @@
%%
clear; close all; clc;
%% Load Plant
load('./mat/G_iff.mat', 'G_iff_light_vc', 'G_iff_light_pz', 'G_iff_heavy_vc', 'G_iff_heavy_pz');
%%
fs = 10;
K_light_vc_iff = generateDiagPidControl(G_iff_light_vc.G_cart, fs);
K_light_pz_iff = generateDiagPidControl(G_iff_light_pz.G_cart, fs);
K_heavy_vc_iff = generateDiagPidControl(G_iff_heavy_vc.G_cart, fs);
K_heavy_pz_iff = generateDiagPidControl(G_iff_heavy_pz.G_cart, fs);
%% Save the MIMO control
save('./mat/K_fb_iff.mat', 'K_light_vc_iff', 'K_light_pz_iff', 'K_heavy_vc_iff', 'K_heavy_pz_iff');

117
hac_lac/hac_lac_iff_control_plots.m Normal file
View File

@ -0,0 +1,117 @@
%%
clear; close all; clc;
%% Load plant and controller
load('./mat/G_iff.mat', 'G_iff_light_vc', 'G_iff_light_pz', 'G_iff_heavy_vc', 'G_iff_heavy_pz');
load('./mat/K_fb_iff.mat', 'K_light_vc_iff', 'K_light_pz_iff', 'K_heavy_vc_iff', 'K_heavy_pz_iff');
%% Load Configuration
load('./mat/config.mat', 'save_fig', 'freqs');
%% Plot the Loop gain for Translations - Light VC
figure;
% Amplitude
ax1 = subaxis(2,1,1);
hold on;
plot(freqs, abs(squeeze(freqresp(K_light_vc_iff(1, 1)*G_iff_light_vc.G_cart(1, 1), freqs, 'Hz'))), 'DisplayName', 'x');
plot(freqs, abs(squeeze(freqresp(K_light_vc_iff(2, 2)*G_iff_light_vc.G_cart(2, 2), freqs, 'Hz'))), 'DisplayName', 'y');
plot(freqs, abs(squeeze(freqresp(K_light_vc_iff(3, 3)*G_iff_light_vc.G_cart(3, 3), freqs, 'Hz'))), 'DisplayName', 'z');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Amplitude [m/N]');
hold off;
% Phase
ax2 = subaxis(2,1,2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(K_light_vc_iff(1, 1)*G_iff_light_vc.G_cart(1, 1), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(K_light_vc_iff(2, 2)*G_iff_light_vc.G_cart(2, 2), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(K_light_vc_iff(3, 3)*G_iff_light_vc.G_cart(3, 3), freqs, 'Hz'))));
set(gca,'xscale','log');
yticks(-180:90:180);
ylim([-180 180]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
legend('Location', 'southwest');
hold off;
linkaxes([ax1,ax2],'x');
if save_fig; exportFig('loop_gain_fb_iff_light_vc_trans', 'normal-normal', struct('path', 'active_damping')); end
%% Plot the Loop gain for Rotations - Light VC
figure;
% Amplitude
ax1 = subaxis(2,1,1);
hold on;
plot(freqs, abs(squeeze(freqresp(K_light_vc_iff(4, 4)*G_iff_light_vc.G_cart(4, 4), freqs, 'Hz'))), 'DisplayName', 'Rx');
plot(freqs, abs(squeeze(freqresp(K_light_vc_iff(5, 5)*G_iff_light_vc.G_cart(5, 5), freqs, 'Hz'))), 'DisplayName', 'Ry');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Amplitude [m/N]');
hold off;
% Phase
ax2 = subaxis(2,1,2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(K_light_vc_iff(4, 4)*G_iff_light_vc.G_cart(4, 4), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(K_light_vc_iff(5, 5)*G_iff_light_vc.G_cart(5, 5), freqs, 'Hz'))));
set(gca,'xscale','log');
yticks(-180:90:180);
ylim([-180 180]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
legend('Location', 'southwest');
hold off;
linkaxes([ax1,ax2],'x');
if save_fig; exportFig('loop_gain_fb_iff_light_vc_rot', 'normal-normal', struct('path', 'active_damping')); end
%% Plot the Loop gain for Translations - Light PZ
figure;
% Amplitude
ax1 = subaxis(2,1,1);
hold on;
plot(freqs, abs(squeeze(freqresp(K_light_pz_iff(1, 1)*G_iff_light_pz.G_cart(1, 1), freqs, 'Hz'))), 'DisplayName', 'x');
plot(freqs, abs(squeeze(freqresp(K_light_pz_iff(2, 2)*G_iff_light_pz.G_cart(2, 2), freqs, 'Hz'))), 'DisplayName', 'y');
plot(freqs, abs(squeeze(freqresp(K_light_pz_iff(3, 3)*G_iff_light_pz.G_cart(3, 3), freqs, 'Hz'))), 'DisplayName', 'z');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Amplitude [m/N]');
hold off;
% Phase
ax2 = subaxis(2,1,2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(K_light_pz_iff(1, 1)*G_iff_light_pz.G_cart(1, 1), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(K_light_pz_iff(2, 2)*G_iff_light_pz.G_cart(2, 2), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(K_light_pz_iff(3, 3)*G_iff_light_pz.G_cart(3, 3), freqs, 'Hz'))));
set(gca,'xscale','log');
yticks(-180:90:180);
ylim([-180 180]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
legend('Location', 'southwest');
hold off;
linkaxes([ax1,ax2],'x');
if save_fig; exportFig('loop_gain_fb_iff_light_pz_trans', 'normal-normal', struct('path', 'active_damping')); end
%% Plot the Loop gain for Rotations - Light PZ
figure;
% Amplitude
ax1 = subaxis(2,1,1);
hold on;
plot(freqs, abs(squeeze(freqresp(K_light_pz_iff(4, 4)*G_iff_light_pz.G_cart(4, 4), freqs, 'Hz'))), 'DisplayName', 'Rx');
plot(freqs, abs(squeeze(freqresp(K_light_pz_iff(5, 5)*G_iff_light_pz.G_cart(5, 5), freqs, 'Hz'))), 'DisplayName', 'Ry');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Amplitude [m/N]');
hold off;
% Phase
ax2 = subaxis(2,1,2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(K_light_pz_iff(4, 4)*G_iff_light_pz.G_cart(4, 4), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(K_light_pz_iff(5, 5)*G_iff_light_pz.G_cart(5, 5), freqs, 'Hz'))));
set(gca,'xscale','log');
yticks(-180:90:180);
ylim([-180 180]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
legend('Location', 'southwest');
hold off;
linkaxes([ax1,ax2],'x');
if save_fig; exportFig('loop_gain_fb_iff_light_pz_rot', 'normal-normal', struct('path', 'active_damping')); end

11
hac_lac/hac_lac_iff_simulation.m Normal file
View File

@ -0,0 +1,11 @@
%%
clear; close all; clc;
%% Initialize Simulation and Inputs
initializeExperiment('tomography', 'light');
%% Run Closed Loop Simulations
runSimulation('vc', 'light', 'cl', 'iff');
runSimulation('pz', 'light', 'cl', 'iff');
% runSimulation('vc', 'heavy', 'cl', 'iff');
% runSimulation('pz', 'heavy', 'cl', 'iff');

2
hac_lac/hac_lac_main.m Normal file
View File

@ -0,0 +1,2 @@
%%
clear; close all; clc;

115
identification/id_G_cart_plots.m Normal file
View File

@ -0,0 +1,115 @@
%%
clear; close all; clc;
%% Load the transfer functions
save('./mat/G.mat', 'G_light_vc', 'G_light_pz', 'G_heavy_vc', 'G_heavy_pz');
%% Load Configuration file
load('./mat/config.mat', 'save_fig', 'freqs');
%% Plant
figure;
% Amplitude
ax1 = subaxis(2,1,1);
hold on;
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_cart('Dx', 'Fnx'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_light_pz.G_cart('Dx', 'Fnx'), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_heavy_vc.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--');
plot(freqs, abs(squeeze(freqresp(G_heavy_pz.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Amplitude [m/N]');
hold off;
% Phase
ax2 = subaxis(2,1,2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_vc.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'DisplayName', 'VC - Light');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_pz.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light');
set(gca,'ColorOrderIndex',1)
plot(freqs, 180/pi*angle(squeeze(freqresp(G_heavy_vc.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_heavy_pz.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy');
set(gca,'xscale','log');
yticks(-1800:90:1800);
ylim([-180 180]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
legend('Location', 'southwest');
hold off;
linkaxes([ax1,ax2],'x');
if save_fig; exportFig('comp_models_plant_x_x', 'normal-normal', struct('path', 'identification')); end
%%
figure;
% Amplitude
ax1 = subaxis(2,1,1);
hold on;
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_cart('Dz', 'Fnz'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_light_pz.G_cart('Dz', 'Fnz'), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_heavy_vc.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--');
plot(freqs, abs(squeeze(freqresp(G_heavy_pz.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Amplitude [m/N]');
hold off;
% Phase
ax2 = subaxis(2,1,2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_vc.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), 'DisplayName', 'VC - Light');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_pz.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light');
set(gca,'ColorOrderIndex',1)
plot(freqs, 180/pi*angle(squeeze(freqresp(G_heavy_vc.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_heavy_pz.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy');
set(gca,'xscale','log');
yticks(-1800:90:1800);
ylim([-180 180]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
legend('Location', 'southwest');
hold off;
linkaxes([ax1,ax2],'x');
if save_fig; exportFig('comp_models_plant_z_z', 'normal-normal', struct('path', 'identification')); end
%% Plot all the coupling
figure;
for i_input = 1:3
for i_output = 1:3
subaxis(3,3,3*(i_input-1)+i_output);
hold on;
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_cart(i_output, i_input), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_light_pz.G_cart(i_output, i_input), freqs, 'Hz'))));
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlim([freqs(1) freqs(end)]); ylim([1e-12, 1e-2]);
yticks([1e-12, 1e-8, 1e-4]); xticks([0.1 1 10 100 1000]);
if i_output > 1; set(gca,'yticklabel',[]); end
if i_input < 3; set(gca,'xticklabel',[]); end
hold off;
end
end
if save_fig; exportFig('comp_models_plant_coupling_all', 'full-tall', struct('path', 'identification')); end
%% Plot some coupling
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'DisplayName', 'VC - Light - $Fx \to Dx$');
plot(freqs, abs(squeeze(freqresp(G_light_pz.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light - $Fx \to Dx$');
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_cart('Dy', 'Fnx'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy - $Fx \to Dy$');
plot(freqs, abs(squeeze(freqresp(G_light_pz.G_cart('Dy', 'Fnx'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy - $Fx \to Dy$');
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_cart('Dz', 'Fnx'), freqs, 'Hz'))), '-.', 'DisplayName', 'VC - Heavy - $Fx \to Dz$');
plot(freqs, abs(squeeze(freqresp(G_light_pz.G_cart('Dz', 'Fnx'), freqs, 'Hz'))), '-.', 'DisplayName', 'PZ - Heavy - $Fx \to Dz$');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/m]');
legend('Location', 'southwest');
xticks('manual'); xlim([freqs(1) freqs(end)]);
hold off;
if save_fig; exportFig('comp_models_plant_coupling', 'normal-normal', struct('path', 'identification')); end

77
identification/id_G_d_plots.m Normal file
View File

@ -0,0 +1,77 @@
%%
clear; close all; clc;
%% Load the identified transfer functions
save('./mat/G.mat', 'G_light_vc', 'G_light_pz', 'G_heavy_vc', 'G_heavy_pz');
%% Load Configuration file
load('./mat/config.mat', 'save_fig', 'freqs');
%% Transfer function from ground displacement to measured displacement
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_dg('Dz', 'Dgz'), freqs, 'Hz'))), 'DisplayName', 'VC - Light');
plot(freqs, abs(squeeze(freqresp(G_light_pz.G_dg('Dz', 'Dgz'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light');
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_heavy_vc.G_dg('Dz', 'Dgz'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy');
plot(freqs, abs(squeeze(freqresp(G_heavy_pz.G_dg('Dz', 'Dgz'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/m]');
hold off;
legend('Location', 'southwest');
if save_fig; exportFig('comp_models_xw_to_d', 'normal-normal', struct('path', 'identification')); end
%%
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_dg('Dx', 'Dgx'), freqs, 'Hz'))), 'DisplayName', 'VC - Light');
plot(freqs, abs(squeeze(freqresp(G_light_pz.G_dg('Dx', 'Dgx'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light');
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_heavy_vc.G_dg('Dx', 'Dgx'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy');
plot(freqs, abs(squeeze(freqresp(G_heavy_pz.G_dg('Dx', 'Dgx'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/m]');
hold off;
legend('Location', 'southwest');
%% Transfer function from direct force to measured displacement
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), 'DisplayName', 'VC - Light');
plot(freqs, abs(squeeze(freqresp(G_light_pz.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light');
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_heavy_vc.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy');
plot(freqs, abs(squeeze(freqresp(G_heavy_pz.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]');
hold off;
legend('Location', 'southwest');
if save_fig; exportFig('comp_models_fi_to_d', 'normal-normal', struct('path', 'identification')); end
%%
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_fs('Ry', 'Fsx'), freqs, 'Hz'))), 'DisplayName', 'VC - Light');
plot(freqs, abs(squeeze(freqresp(G_light_pz.G_fs('Ry', 'Fsx'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light');
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_heavy_vc.G_fs('Ry', 'Fsx'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy');
plot(freqs, abs(squeeze(freqresp(G_heavy_pz.G_fs('Ry', 'Fsx'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]');
hold off;
legend('Location', 'southwest');
%%
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_fs('Rz', 'Fsx'), freqs, 'Hz'))), 'DisplayName', 'VC - Light');
plot(freqs, abs(squeeze(freqresp(G_light_pz.G_fs('Rz', 'Fsx'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light');
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_heavy_vc.G_fs('Rz', 'Fsx'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy');
plot(freqs, abs(squeeze(freqresp(G_heavy_pz.G_fs('Rz', 'Fsx'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]');
hold off;
legend('Location', 'southwest');

57
identification/id_G_iff_plots.m Normal file
View File

@ -0,0 +1,57 @@
%%
clear; close all; clc;
%% Load the identified transfer functions
load('./mat/G.mat', 'G_light_vc', 'G_light_pz', 'G_heavy_vc', 'G_heavy_pz');
%% Load Configuration file
load('./mat/config.mat', 'save_fig', 'freqs');
%%
figure;
% Amplitude
ax1 = subaxis(2,1,1);
hold on;
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_iff('Fm1', 'F1'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_light_pz.G_iff('Fm1', 'F1'), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_heavy_vc.G_iff('Fm1', 'F1'), freqs, 'Hz'))), '--');
plot(freqs, abs(squeeze(freqresp(G_heavy_pz.G_iff('Fm1', 'F1'), freqs, 'Hz'))), '--');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Amplitude [m/N]');
hold off;
% Phase
ax2 = subaxis(2,1,2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_vc.G_iff('Fm1', 'F1'), freqs, 'Hz'))), 'DisplayName', 'VC - Light');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_pz.G_iff('Fm1', 'F1'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light');
set(gca,'ColorOrderIndex',1)
plot(freqs, 180/pi*angle(squeeze(freqresp(G_heavy_vc.G_iff('Fm1', 'F1'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_heavy_pz.G_iff('Fm1', 'F1'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy');
set(gca,'xscale','log');
yticks(-180:90:180);
ylim([-180 180]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
legend('Location', 'southwest');
hold off;
linkaxes([ax1,ax2],'x');
if save_fig; exportFig('G_iff', 'normal-normal', struct('path', 'identification')); end
%% Coupling
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_iff('Fm1', 'F1'), freqs, 'Hz'))), 'k-');
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_iff('Fm2', 'F1'), freqs, 'Hz'))), 'k--');
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_iff('Fm3', 'F1'), freqs, 'Hz'))), 'k--');
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_iff('Fm4', 'F1'), freqs, 'Hz'))), 'k--');
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_iff('Fm5', 'F1'), freqs, 'Hz'))), 'k--');
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_iff('Fm6', 'F1'), freqs, 'Hz'))), 'k--');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
hold off;
if save_fig; exportFig('G_iff_coupling', 'normal-normal', struct('path', 'identification')); end

115
identification/id_G_plots.m
View File

@ -1,115 +0,0 @@
%%
clear; close all; clc;
%% Load the transfer functions
load('./mat/G_f_to_d.mat', 'G_1_vc', 'G_1_pz', 'G_50_vc', 'G_50_pz');
%% Load Configuration file
load('./mat/config.mat', 'save_fig', 'freqs');
%%
figure;
% Amplitude
ax1 = subaxis(2,1,1);
hold on;
plot(freqs, abs(squeeze(freqresp(G_1_vc('Dx', 'Fnx'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_1_pz('Dx', 'Fnx'), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_50_vc('Dx', 'Fnx'), freqs, 'Hz'))), '--');
plot(freqs, abs(squeeze(freqresp(G_50_pz('Dx', 'Fnx'), freqs, 'Hz'))), '--');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Amplitude [m/N]');
hold off;
% Phase
ax2 = subaxis(2,1,2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G_1_vc('Dx', 'Fnx'), freqs, 'Hz'))), 'DisplayName', 'VC - Light');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_1_pz('Dx', 'Fnx'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light');
set(gca,'ColorOrderIndex',1)
plot(freqs, 180/pi*angle(squeeze(freqresp(G_50_vc('Dx', 'Fnx'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_50_pz('Dx', 'Fnx'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy');
set(gca,'xscale','log');
yticks(-1800:90:1800);
ylim([-180 180]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
legend('Location', 'southwest');
hold off;
linkaxes([ax1,ax2],'x');
if save_fig; exportFig('comp_models_plant_x_x', 'normal-normal', struct('path', 'identification')); end
%%
figure;
% Amplitude
ax1 = subaxis(2,1,1);
hold on;
plot(freqs, abs(squeeze(freqresp(G_1_vc('Dz', 'Fnz'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_1_pz('Dz', 'Fnz'), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_50_vc('Dz', 'Fnz'), freqs, 'Hz'))), '--');
plot(freqs, abs(squeeze(freqresp(G_50_pz('Dz', 'Fnz'), freqs, 'Hz'))), '--');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Amplitude [m/N]');
hold off;
% Phase
ax2 = subaxis(2,1,2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G_1_vc('Dz', 'Fnz'), freqs, 'Hz'))), 'DisplayName', 'VC - Light');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_1_pz('Dz', 'Fnz'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light');
set(gca,'ColorOrderIndex',1)
plot(freqs, 180/pi*angle(squeeze(freqresp(G_50_vc('Dz', 'Fnz'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_50_pz('Dz', 'Fnz'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy');
set(gca,'xscale','log');
yticks(-1800:90:1800);
ylim([-180 180]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
legend('Location', 'southwest');
hold off;
linkaxes([ax1,ax2],'x');
if save_fig; exportFig('comp_models_plant_z_z', 'normal-normal', struct('path', 'identification')); end
%% Plot all the coupling
figure;
for i_input = 1:3
for i_output = 1:3
subaxis(3,3,3*(i_input-1)+i_output);
hold on;
plot(freqs, abs(squeeze(freqresp(G_1_vc(i_output, i_input), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_1_pz(i_output, i_input), freqs, 'Hz'))));
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlim([freqs(1) freqs(end)]); ylim([1e-12, 1e-2]);
yticks([1e-12, 1e-8, 1e-4]); xticks([0.1 1 10 100 1000]);
if i_output > 1; set(gca,'yticklabel',[]); end
if i_input < 3; set(gca,'xticklabel',[]); end
hold off;
end
end
if save_fig; exportFig('comp_models_plant_coupling_all', 'full-tall', struct('path', 'identification')); end
%% Plot some coupling
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_1_vc('Dx', 'Fnx'), freqs, 'Hz'))), 'DisplayName', 'VC - Light - $Fx \to Dx$');
plot(freqs, abs(squeeze(freqresp(G_1_pz('Dx', 'Fnx'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light - $Fx \to Dx$');
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_1_vc('Dy', 'Fnx'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy - $Fx \to Dy$');
plot(freqs, abs(squeeze(freqresp(G_1_pz('Dy', 'Fnx'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy - $Fx \to Dy$');
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_1_vc('Dz', 'Fnx'), freqs, 'Hz'))), '-.', 'DisplayName', 'VC - Heavy - $Fx \to Dz$');
plot(freqs, abs(squeeze(freqresp(G_1_pz('Dz', 'Fnx'), freqs, 'Hz'))), '-.', 'DisplayName', 'PZ - Heavy - $Fx \to Dz$');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/m]');
legend('Location', 'southwest');
xticks('manual'); xlim([freqs(1) freqs(end)]);
hold off;
if save_fig; exportFig('comp_models_plant_coupling', 'normal-normal', struct('path', 'identification')); end

28
identification/id_Gd.m
View File

@ -1,28 +0,0 @@
%% Script Description
% Identification of the transfer function
% from Ground Motion to measured displacement
%%
clear; close all; clc;
%% Open Loop - Light Sample
initializeSample(struct('mass', 1));
initializeNanoHexapod(struct('actuator', 'lorentz'));
Gd_ol_1_vc = identifyGd(struct('cl', false));
initializeNanoHexapod(struct('actuator', 'piezo'));
Gd_ol_1_pz = identifyGd(struct('cl', false));
%% Open Loop - Heavy Sample
initializeSample(struct('mass', 50));
initializeNanoHexapod(struct('actuator', 'lorentz'));
Gd_ol_50_vc = identifyGd(struct('cl', false));
initializeNanoHexapod(struct('actuator', 'piezo'));
Gd_ol_50_pz = identifyGd(struct('cl', false));
%% Save the identified transfer functions
save('./mat/G_xw_to_d.mat', ...
'Gd_ol_1_vc', 'Gd_ol_1_pz', 'Gd_ol_50_vc', 'Gd_ol_50_pz');

39
identification/id_Gd_plots.m
View File

@ -1,39 +0,0 @@
%%
clear; close all; clc;
%% Load the identified transfer functions
load('./mat/G_xw_to_d.mat', ...
'Gd_ol_1_vc', 'Gd_ol_1_pz', 'Gd_ol_50_vc', 'Gd_ol_50_pz');
%% Load Configuration file
load('./mat/config.mat', 'save_fig', 'freqs');
%% Transfer function from ground displacement to measured displacement
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(Gd_ol_1_vc('Dz', 'Dgz'), freqs, 'Hz'))), 'DisplayName', 'VC - Light');
plot(freqs, abs(squeeze(freqresp(Gd_ol_1_pz('Dz', 'Dgz'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light');
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gd_ol_50_vc('Dz', 'Dgz'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy');
plot(freqs, abs(squeeze(freqresp(Gd_ol_50_pz('Dz', 'Dgz'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/m]');
hold off;
legend('Location', 'southwest');
if save_fig; exportFig('comp_models_xw_to_d', 'normal-normal', struct('path', 'identification')); end
%% Transfer function from direct force to measured displacement
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(Gd_ol_1_vc('Dz', 'Fsz'), freqs, 'Hz'))), 'DisplayName', 'VC - Light');
plot(freqs, abs(squeeze(freqresp(Gd_ol_1_pz('Dz', 'Fsz'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light');
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gd_ol_50_vc('Dz', 'Fsz'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy');
plot(freqs, abs(squeeze(freqresp(Gd_ol_50_pz('Dz', 'Fsz'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]');
hold off;
legend('Location', 'southwest');
if save_fig; exportFig('comp_models_fi_to_d', 'normal-normal', struct('path', 'identification')); end

27
identification/id_main.m
View File

@ -1,20 +1,6 @@
%%
clear; close all; clc;
%% Plant Identification
% Compute the transfer function of G for multiple masses
run id_G.m
% Plot de obtained transfer functions
run id_G_plots.m
%% Identification of transfer function from disturbances to displacement
% Compute the transfer function of Gd
run id_Gd.m
% Plot de obtained transfer functions
run id_Gd_plots.m
%% Identification of the micro-station
% Compute the transfer functions
run id_micro_station.m
@ -25,6 +11,19 @@ run id_micro_station_plots.m
% Compare the measurements of Marc with the model
run id_micro_station_comp_meas.m
%% Identification of the nano-station
% Run the identification
run id_nano_station.m
% Plot the plant for feedback control
run id_G_cart_plots.m
% Plot the transfer function from disturbances to displacement
run id_G_d_plots.m
% Plot the transfer function for IFF control
run id_G_iff_plots.m
%% Identification of all the stages
% Compute the transfer functions of each stage from act. to sens.
run id_stages.m

25
identification/id_G.m → identification/id_nano_station.m
View File

@ -1,28 +1,27 @@
%% Script Description
% Identification of a force injected into the NASS (in cartesian
% coordinates) to the relative displacement of the sample
% and granite.
%%
clear; close all; clc;
%%
initializeNanoHexapod(struct('actuator', 'lorentz'));
K_iff = tf(zeros(6));
save('./mat/K_iff.mat', 'K_iff');
%% Light Sample
initializeSample(struct('mass', 1));
G_1_vc = identifyG();
initializeNanoHexapod(struct('actuator', 'lorentz'));
G_light_vc = identifyPlant();
initializeNanoHexapod(struct('actuator', 'piezo'));
G_1_pz = identifyG();
G_light_pz = identifyPlant();
%%
initializeNanoHexapod(struct('actuator', 'lorentz'));
%% Heavy Sample
initializeSample(struct('mass', 50));
G_50_vc = identifyG();
initializeNanoHexapod(struct('actuator', 'lorentz'));
G_heavy_vc = identifyPlant();
initializeNanoHexapod(struct('actuator', 'piezo'));
G_50_pz = identifyG();
G_heavy_pz = identifyPlant();
%% Save the obtained transfer functions
save('./mat/G_f_to_d.mat', 'G_1_vc', 'G_1_pz', 'G_50_vc', 'G_50_pz');
save('./mat/G.mat', 'G_light_vc', 'G_light_pz', 'G_heavy_vc', 'G_heavy_pz');

BIN
identification/sim_nano_station.slx
View File

Binary file not shown.

1
init_simulation.m
View File

@ -23,3 +23,4 @@ load('./mat/inputs.mat', 'inputs');
%% Load Controller
load('./mat/controller.mat', 'K');
load('./mat/K_iff.mat', 'K_iff');

25
initialize/initializeExperiment.m Normal file
View File

@ -0,0 +1,25 @@
function [] = initializeExperiment(exp_name, sys_mass)
if strcmp(exp_name, 'tomography')
opts_sim = struct(...
'Tsim', 5, ...
'cl_time', 5 ...
);
initializeSimConf(opts_sim);
if strcmp(sys_mass, 'light')
opts_inputs = struct(...
'ground_motion', true, ...
'rz', 60 ... % rpm
);
elseif strcpm(sys_mass, 'heavy')
opts_inputs = struct(...
'ground_motion', true, ...
'rz', 1 ... % rpm
);
else
error('sys_mass should be light or heavy');
end
initializeInputs(opts_inputs);
elseif
end

10
initialize/initializeInputs.m
View File

@ -112,22 +112,14 @@ function [inputs] = initializeInputs(opts_param)
%% Set point [m, rad]
if islogical(opts.setpoint) && opts.setpoint == true
setpoint = zeros(length(time_vector), 6);
setpoint(ceil(10/sim_conf.Ts):end, 2) = 1e-6; % Step of 1 micro-meter in y direction
elseif islogical(opts.setpoint) && opts.setpoint == false
setpoint = zeros(length(time_vector), 6);
else
setpoint = opts.setpoint;
end
% The setpoint in rotation should be the same as the rotation of the Spindle
% Should change that. And think how to include all the setpoint of each stage in this
% global setpoint. Maybe do everything in simulink
setpoint(:, 6) = rz;
inputs.setpoint = timeseries(setpoint, time_vector);
%% Save if no output argument
if nargout == 0
%% Save
save('./mat/inputs.mat', 'inputs');
end
end

8
initialize/initializeSimConf.m
View File

@ -3,7 +3,7 @@ function [] = initializeSimConf(opts_param)
opts = struct('Ts', 1e-4, ... % Sampling time [s]
'Tsim', 10, ... % Simulation time [s]
'cl_time', 0, ... % Close Loop time [s]
'gravity', false ... % Gravity along the z axis [m/s^2]
'gravity', false ... % Gravity along the z axis
);
%% Populate opts with input parameters
@ -23,13 +23,11 @@ function [] = initializeSimConf(opts_param)
%% Gravity
if opts.gravity
sim_conf.g = -9.8;
sim_conf.g = -9.8; %#ok
else
sim_conf.g = 0;
sim_conf.g = 0; %#ok
end
%% Save
save('./mat/sim_conf.mat', 'sim_conf');
end

30
initialize/initializeSmiData.m
View File

@ -1,6 +1,6 @@
function [smiData] = initializeSmiData()
%% Initialize the structure
smiData = struct;
smiData = struct();
%% Rigid Transform
smiData.RigidTransform = struct;
@ -265,6 +265,30 @@ function [smiData] = initializeSmiData()
smiData.RigidTransform(65).axis = [-0.57735026918962584 -0.57735026918962584 -0.57735026918962584];
smiData.RigidTransform(65).ID = 'B[Guide_Tilt-2:-:Plateau_Tilt-1]';
smiData.RigidTransform(66).translation = [-313.5 0 0];
smiData.RigidTransform(66).angle = 2.0943951023931962;
smiData.RigidTransform(66).axis = [-0.577350269189626 -0.57735026918962606 0.5773502691896254];
smiData.RigidTransform(66).ID = 'F[Guide_Tilt-2:-:Plateau_Tilt-1]';
smiData.RigidTransform(67).translation = [0 -5 600];
smiData.RigidTransform(67).angle = 2.0943951023931953;
smiData.RigidTransform(67).axis = [-0.57735026918962584 -0.57735026918962584 -0.57735026918962584];
smiData.RigidTransform(67).ID = 'B[Guide_Tilt-3:-:Plateau_Tilt-1]';
smiData.RigidTransform(68).translation = [313.5 0 0];
smiData.RigidTransform(68).angle = 2.0943951023931948;
smiData.RigidTransform(68).axis = [0.57735026918962551 0.57735026918962562 0.57735026918962629];
smiData.RigidTransform(68).ID = 'F[Guide_Tilt-3:-:Plateau_Tilt-1]';
smiData.RigidTransform(69).translation = [0 -5 600];
smiData.RigidTransform(69).angle = 2.0943951023931953;
smiData.RigidTransform(69).axis = [-0.57735026918962584 -0.57735026918962584 -0.57735026918962584];
smiData.RigidTransform(69).ID = 'B[Guide_Tilt-4:-:Plateau_Tilt-1]';
smiData.RigidTransform(70).translation = [313.5 0 0];
smiData.RigidTransform(70).angle = 2.0943951023931948;
smiData.RigidTransform(70).axis = [0.57735026918962551 0.57735026918962562 0.57735026918962629];
smiData.RigidTransform(70).ID = 'F[Guide_Tilt-4:-:Plateau_Tilt-1]';
smiData.RigidTransform(71).translation = [146.02 0 0];
smiData.RigidTransform(71).angle = 2.0943951023931953;
smiData.RigidTransform(71).axis = [0.57735026918962584 0.57735026918962584 0.57735026918962584];
smiData.RigidTransform(71).ID = 'B[Bati_Spindle-1:-:Axe_Spindle-1]';
smiData.RigidTransform(72).translation = [146.2 0 0];
smiData.RigidTransform(72).angle = 2.0943951023931957;
smiData.RigidTransform(72).axis = [0.57735026918962573 0.57735026918962584 0.57735026918962573];
smiData.RigidTransform(72).ID = 'F[Bati_Spindle-1:-:Axe_Spindle-1]';
@ -1348,8 +1372,6 @@ function [smiData] = initializeSmiData()
smiData.SphericalJoint(24).S.Pos.Axis = [0.77690891930122863 0.35378802664966663 0.52081336706111148];
smiData.SphericalJoint(24).ID = '[BrasHaut_Nano-6:-:Nacelle_Nano_Support-1]';
%% If no output argument, save the object
if nargout == 0
%% Save
save('./mat/smiData.mat', 'smiData')
end
end

17
main.m
View File

@ -4,9 +4,6 @@ clear; close all; clc;
%% Open the project
simulinkproject('./');
%% General Configuration
run config.m
%% Initialization
% Initialize the perturbations
run init_perturbations.m
@ -14,11 +11,17 @@ run init_perturbations.m
% Initialize all the stages parameters
run init_data.m
%% Run the simulations
run run_simulations.m
%% Demonstration of displacement of all the stages
run init_data_demonstration.m
run demonstration_main.m
%% Identification
open id_main.m
%% Active Damping Control
open act_damp_main.m
%% Control With the Undamped System
open control_main.m
%% HAC-LAC Control
open hac_lac_main.m

BIN
mat/G.mat Normal file
View File

Binary file not shown.

BIN
mat/G_f_to_d.mat
View File

Binary file not shown.

BIN
mat/G_iff.mat Normal file
View File

Binary file not shown.

BIN
mat/K_fb.mat Normal file
View File

Binary file not shown.

BIN
mat/K_fb_iff.mat Normal file
View File

Binary file not shown.

BIN
mat/K_iff.mat Normal file
View File

Binary file not shown.

BIN
mat/K_iff_crit.mat Normal file
View File

Binary file not shown.

BIN
mat/axisc.mat
View File

Binary file not shown.

BIN
mat/config.mat Normal file
View File

Binary file not shown.

BIN
mat/controller.mat
View File

Binary file not shown.

BIN
mat/granite.mat
View File

Binary file not shown.

BIN
mat/ground.mat
View File

Binary file not shown.

BIN
mat/id_micro_station.mat Normal file
View File

Binary file not shown.

BIN
mat/inputs.mat
View File

Binary file not shown.

BIN
mat/micro_hexapod.mat
View File

Binary file not shown.

BIN
mat/mirror.mat
View File

Binary file not shown.

BIN
mat/nano_hexapod.mat
View File

Binary file not shown.

BIN
mat/ry.mat
View File

Binary file not shown.

BIN
mat/rz.mat
View File

Binary file not shown.

BIN
mat/sample.mat
View File

Binary file not shown.

BIN
mat/sim_conf.mat
View File

Binary file not shown.

BIN
mat/smiData.mat
View File

Binary file not shown.

BIN
mat/stewart.mat Normal file
View File

Binary file not shown.

BIN
mat/ty.mat
View File

Binary file not shown.

23
src/computePsdDispl.m Normal file
View File

@ -0,0 +1,23 @@
function [psd_object] = computePsdDispl(sys_data, t_init, n_av)
i_init = find(sys_data.time > t_init, 1);
han_win = hanning(ceil(length(sys_data.Dx(i_init:end, :))/n_av));
Fs = 1/sys_data.time(2);
[pdx, f] = pwelch(sys_data.Dx(i_init:end, :), han_win, [], [], Fs);
[pdy, ~] = pwelch(sys_data.Dy(i_init:end, :), han_win, [], [], Fs);
[pdz, ~] = pwelch(sys_data.Dz(i_init:end, :), han_win, [], [], Fs);
[prx, ~] = pwelch(sys_data.Rx(i_init:end, :), han_win, [], [], Fs);
[pry, ~] = pwelch(sys_data.Ry(i_init:end, :), han_win, [], [], Fs);
[prz, ~] = pwelch(sys_data.Rz(i_init:end, :), han_win, [], [], Fs);
psd_object = struct(...
'f', f, ...
'dx', pdx, ...
'dy', pdy, ...
'dz', pdz, ...
'rx', prx, ...
'ry', pry, ...
'rz', prz);
end

18
src/generateDiagPidControl.m Normal file
View File

@ -0,0 +1,18 @@
function [K] = generateDiagPidControl(G, fs)
%%
pid_opts = pidtuneOptions(...
'PhaseMargin', 50, ...
'DesignFocus', 'disturbance-rejection');
%%
K = tf(zeros(6));
for i = 1:5
input_name = G.InputName(i);
output_name = G.OutputName(i);
K(i, i) = tf(pidtune(minreal(G(output_name, input_name)), 'PIDF', 2*pi*fs, pid_opts));
end
K.InputName = G.OutputName;
K.OutputName = G.InputName;
end

28
src/identifyG.m
View File

@ -1,28 +0,0 @@
function [G, G_raw] = identifyG(opts_param)
%% Default values for opts
opts = struct();
%% Populate opts with input parameters
if exist('opts_param','var')
for opt = fieldnames(opts_param)'
opts.(opt{1}) = opts_param.(opt{1});
end
end
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'sim_nano_station';
%% Centralized control (Cartesian coordinates)
% Input/Output definition
io(1) = linio([mdl, '/Micro-Station/Fn'], 1,'openinput');
io(2) = linio([mdl, '/Micro-Station/Sample'],1,'output');
% Run the linearization
G = linearize(mdl,io, 0);
G.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'};
G.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
end

35
src/identifyGd.m
View File

@ -1,35 +0,0 @@
function [Gd, Gd_raw] = identifyGd(opts_param)
%% Default values for opts
opts = struct('cl', true);
%% Populate opts with input parameters
if exist('opts_param','var')
for opt = fieldnames(opts_param)'
opts.(opt{1}) = opts_param.(opt{1});
end
end
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
if opts.cl
% Make sure that the loop is closed
initializeSimConf(struct('cl_time', 0));
mdl = 'Assemblage';
else
mdl = 'sim_nano_station';
end
%% Centralized control (Cartesian coordinates)
% Input/Output definition
io(1) = linio([mdl, '/Micro-Station/Gm'], 1,'openinput');
io(2) = linio([mdl, '/Micro-Station/Fs_ext'], 1,'openinput');
io(3) = linio([mdl, '/Micro-Station/Sample'], 1,'output');
% Run the linearization
Gd = linearize(mdl,io, 0);
Gd.InputName = {'Dgx', 'Dgy', 'Dgz', 'Fsx', 'Fsy', 'Fsz'};
Gd.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
end

30
src/identifyNass.m
View File

@ -1,30 +0,0 @@
function [G_cart, G_cart_raw] = identifyNass(opts_param)
%% Default values for opts
opts = struct();
%% Populate opts with input parameters
if exist('opts_param','var')
for opt = fieldnames(opts_param)'
opts.(opt{1}) = opts_param.(opt{1});
end
end
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'sim_nano_station';
%% Centralized control (Cartesian coordinates)
% Input/Output definition
io(1) = linio([mdl, '/Micro-Station/Fn'], 1, 'openinput');
io(2) = linio([mdl, '/Micro-Station/Nano_Hexapod'], 1, 'output');
% Run the linearization
G_cart = linearize(mdl,io, 0);
% Input/Output names
G_cart.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'};
G_cart.OutputName = {'Dnx', 'Dny', 'Dnz', 'Rnx', 'Rny', 'Rnz'};
end

42
src/identifyPlant.m Normal file
View File

@ -0,0 +1,42 @@
function [sys] = identifyPlant(opts_param)
%% Default values for opts
opts = struct();
%% Populate opts with input parameters
if exist('opts_param','var')
for opt = fieldnames(opts_param)'
opts.(opt{1}) = opts_param.(opt{1});
end
end
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'sim_nano_station';
%% Input/Output definition
io(1) = linio([mdl, '/Fn'], 1, 'input');
io(2) = linio([mdl, '/Gm'], 1, 'input');
io(3) = linio([mdl, '/Fs_ext'], 1, 'input');
io(4) = linio([mdl, '/F_legs'], 1, 'input');
io(5) = linio([mdl, '/Dsample_meas'], 1, 'output');
io(6) = linio([mdl, '/F_meas'], 1, 'output');
%% Run the linearization
G = linearize(mdl, io, 0);
G.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz', ...
'Dgx', 'Dgy', 'Dgz', ...
'Fsx', 'Fsy', 'Fsz', ...
'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', ...
'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6'};
%% Create the sub transfer functions
sys.G_cart = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'}));
sys.G_gm = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Dgx', 'Dgy', 'Dgz'}));
sys.G_fs = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Fsx', 'Fsy', 'Fsz'}));
sys.G_iff = minreal(G({'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}));
end

56
src/runSimulation.m Normal file
View File

@ -0,0 +1,56 @@
function [] = runSimulation(sys_name, sys_mass, ctrl_type, act_damp)
%% Load the controller and save it for the simulation
if strcmp(ctrl_type, 'cl') && strcmp(act_damp, 'none')
K_obj = load('./mat/K_fb.mat');
K = K_obj.(sprintf('K_%s_%s', sys_mass, sys_name)); %#ok
save('./mat/controller.mat', 'K');
elseif strcmp(ctrl_type, 'cl') && strcmp(act_damp, 'iff')
K_obj = load('./mat/K_fb_iff.mat');
K = K_obj.(sprintf('K_%s_%s_iff', sys_mass, sys_name)); %#ok
save('./mat/controller.mat', 'K');
elseif strcmp(ctrl_type, 'ol')
K = tf(zeros(6)); %#ok
save('./mat/controller.mat', 'K');
else
error('ctrl_type should be cl or ol');
end
%% Active Damping
if strcmp(act_damp, 'iff')
K_iff_crit = load('./mat/K_iff_crit.mat');
K_iff = K_iff_crit.(sprintf('K_iff_%s_%s', sys_mass, sys_name)); %#ok
save('./mat/K_iff.mat', 'K_iff');
elseif strcmp(act_damp, 'none')
K_iff = tf(zeros(6)); %#ok
save('./mat/K_iff.mat', 'K_iff');
end
%%
if strcmp(sys_name, 'pz')
initializeNanoHexapod(struct('actuator', 'piezo'));
elseif strcmp(sys_name, 'vc')
initializeNanoHexapod(struct('actuator', 'lorentz'));
else
error('sys_name should be pz or vc');
end
if strcmp(sys_mass, 'light')
initializeSample(struct('mass', 1));
elseif strcmp(sys_mass, 'heavy')
initializeSample(struct('mass', 50));
else
error('sys_mass should be light or heavy');
end
%% Run the simulation
sim('Assemblage.slx');
%% Split the Dsample matrix into vectors
[Dx, Dy, Dz, Rx, Ry, Rz] = matSplit(Dsample.Data, 1); %#ok
time = Dsample.Time; %#ok
%% Save the result
filename = sprintf('sim_%s_%s_%s_%s', sys_mass, sys_name, ctrl_type, act_damp);
save(sprintf('./mat/%s.mat', filename), ...
'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
end

2
stewart-simscape

@ -1 +1 @@
Subproject commit bef54b5bf7967d8eb61a2d24d39a729e5d34c76d
Subproject commit 9d1250990137ddc246a1d7bf4084b36377cc55a3