diff --git a/.SimulinkProject/Root.type.ProjectPath/1809d361-dc3d-4fcb-8b9b-833180e2f6fc.type.Reference.xml b/.SimulinkProject/Root.type.ProjectPath/1809d361-dc3d-4fcb-8b9b-833180e2f6fc.type.Reference.xml new file mode 100644 index 0000000..1c14e65 --- /dev/null +++ b/.SimulinkProject/Root.type.ProjectPath/1809d361-dc3d-4fcb-8b9b-833180e2f6fc.type.Reference.xml @@ -0,0 +1,2 @@ + + \ 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 deleted file mode 100644 index 6fb784a..0000000 --- a/.SimulinkProject/Root.type.ProjectPath/2793b451-85f9-44a8-8d10-25d7657da4e0.type.Reference.xml +++ /dev/null @@ -1,2 +0,0 @@ - - \ No newline at end of file diff --git a/.SimulinkProject/Root.type.ProjectPath/2d80ad84-a2e6-469a-be37-c276ae1af074.type.Reference.xml b/.SimulinkProject/Root.type.ProjectPath/56b46427-db89-446e-ab1b-3a8188f5e1b3.type.Reference.xml similarity index 50% rename from .SimulinkProject/Root.type.ProjectPath/2d80ad84-a2e6-469a-be37-c276ae1af074.type.Reference.xml rename to .SimulinkProject/Root.type.ProjectPath/56b46427-db89-446e-ab1b-3a8188f5e1b3.type.Reference.xml index 572863f..702b119 100644 --- a/.SimulinkProject/Root.type.ProjectPath/2d80ad84-a2e6-469a-be37-c276ae1af074.type.Reference.xml +++ b/.SimulinkProject/Root.type.ProjectPath/56b46427-db89-446e-ab1b-3a8188f5e1b3.type.Reference.xml @@ -1,2 +1,2 @@ - \ No newline at end of file + \ 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/7e53593c-166d-46eb-8350-609f08763240.type.Reference.xml similarity index 50% rename from .SimulinkProject/Root.type.ProjectPath/c767e49c-8d99-42c0-ab3b-d872f82e48d2.type.Reference.xml rename to .SimulinkProject/Root.type.ProjectPath/7e53593c-166d-46eb-8350-609f08763240.type.Reference.xml index 0c381b2..04a176b 100644 --- a/.SimulinkProject/Root.type.ProjectPath/c767e49c-8d99-42c0-ab3b-d872f82e48d2.type.Reference.xml +++ b/.SimulinkProject/Root.type.ProjectPath/7e53593c-166d-46eb-8350-609f08763240.type.Reference.xml @@ -1,2 +1,2 @@ - \ No newline at end of file + \ No newline at end of file diff --git a/.SimulinkProject/Root.type.ProjectPath/83f0e724-9931-453a-b699-3bd293db000c.type.Reference.xml b/.SimulinkProject/Root.type.ProjectPath/83f0e724-9931-453a-b699-3bd293db000c.type.Reference.xml new file mode 100644 index 0000000..ef68a83 --- /dev/null +++ b/.SimulinkProject/Root.type.ProjectPath/83f0e724-9931-453a-b699-3bd293db000c.type.Reference.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/.SimulinkProject/Root.type.ProjectPath/a1959b94-b957-42df-87b8-4945482358a6.type.Reference.xml b/.SimulinkProject/Root.type.ProjectPath/a1959b94-b957-42df-87b8-4945482358a6.type.Reference.xml new file mode 100644 index 0000000..5c6b9cb --- /dev/null +++ b/.SimulinkProject/Root.type.ProjectPath/a1959b94-b957-42df-87b8-4945482358a6.type.Reference.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/.SimulinkProject/Root.type.ProjectPath/d03bc20c-98e3-4ed4-a859-51e1f5a18637.type.Reference.xml b/.SimulinkProject/Root.type.ProjectPath/d03bc20c-98e3-4ed4-a859-51e1f5a18637.type.Reference.xml new file mode 100644 index 0000000..29ae5de --- /dev/null +++ b/.SimulinkProject/Root.type.ProjectPath/d03bc20c-98e3-4ed4-a859-51e1f5a18637.type.Reference.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/Assemblage.slx b/Assemblage.slx index 04c85eb..152a489 100644 Binary files a/Assemblage.slx and b/Assemblage.slx differ diff --git a/Control/cl_tf.m b/Control/cl_tf.m deleted file mode 100644 index dafd902..0000000 --- a/Control/cl_tf.m +++ /dev/null @@ -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'); diff --git a/Control/generate_control.m b/Control/generate_control.m deleted file mode 100644 index 6685a1b..0000000 --- a/Control/generate_control.m +++ /dev/null @@ -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'); diff --git a/Micro_Station_Displacement.slx b/Micro_Station_Displacement.slx deleted file mode 100644 index fc3abea..0000000 Binary files a/Micro_Station_Displacement.slx and /dev/null differ diff --git a/active_damping/act_damp_main.m b/active_damping/act_damp_main.m new file mode 100644 index 0000000..41228e0 --- /dev/null +++ b/active_damping/act_damp_main.m @@ -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 diff --git a/active_damping/dvf_control.m b/active_damping/dvf_control.m new file mode 100644 index 0000000..8b79900 --- /dev/null +++ b/active_damping/dvf_control.m @@ -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'); diff --git a/active_damping/iff_comp_tf.m b/active_damping/iff_comp_tf.m new file mode 100644 index 0000000..0559cc6 --- /dev/null +++ b/active_damping/iff_comp_tf.m @@ -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 diff --git a/active_damping/iff_control.m b/active_damping/iff_control.m new file mode 100644 index 0000000..a16c2ff --- /dev/null +++ b/active_damping/iff_control.m @@ -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'); \ No newline at end of file diff --git a/active_damping/iff_identification.m b/active_damping/iff_identification.m new file mode 100644 index 0000000..c2db8db --- /dev/null +++ b/active_damping/iff_identification.m @@ -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'); diff --git a/active_damping/iff_results.m b/active_damping/iff_results.m new file mode 100644 index 0000000..afcf864 --- /dev/null +++ b/active_damping/iff_results.m @@ -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)) diff --git a/active_damping/iff_simulation.m b/active_damping/iff_simulation.m new file mode 100644 index 0000000..d501c5d --- /dev/null +++ b/active_damping/iff_simulation.m @@ -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'); diff --git a/config.m b/config.m index f8c8d17..7099b22 100644 --- a/config.m +++ b/config.m @@ -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); diff --git a/control/control_cl_ol_plots.m b/control/control_cl_ol_plots.m new file mode 100644 index 0000000..50338ef --- /dev/null +++ b/control/control_cl_ol_plots.m @@ -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 diff --git a/control/control_cl_sim.m b/control/control_cl_sim.m new file mode 100644 index 0000000..0b32bd1 --- /dev/null +++ b/control/control_cl_sim.m @@ -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'); diff --git a/control/control_cl_tf.m b/control/control_cl_tf.m new file mode 100644 index 0000000..e26af84 --- /dev/null +++ b/control/control_cl_tf.m @@ -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'); \ No newline at end of file diff --git a/control/control_cl_tf_comp.m b/control/control_cl_tf_comp.m new file mode 100644 index 0000000..4647f24 --- /dev/null +++ b/control/control_cl_tf_comp.m @@ -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 \ No newline at end of file diff --git a/control/control_generate.m b/control/control_generate.m new file mode 100644 index 0000000..cd42114 --- /dev/null +++ b/control/control_generate.m @@ -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'); diff --git a/control/control_main.m b/control/control_main.m new file mode 100644 index 0000000..ff6cb9f --- /dev/null +++ b/control/control_main.m @@ -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 diff --git a/control/control_ol_psd.m b/control/control_ol_psd.m new file mode 100644 index 0000000..cf4165e --- /dev/null +++ b/control/control_ol_psd.m @@ -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 diff --git a/control/control_ol_sim.m b/control/control_ol_sim.m new file mode 100644 index 0000000..9849e13 --- /dev/null +++ b/control/control_ol_sim.m @@ -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'); diff --git a/demonstration/Micro_Station_Displacement.slx b/demonstration/Micro_Station_Displacement.slx new file mode 100644 index 0000000..44d4e0c Binary files /dev/null and b/demonstration/Micro_Station_Displacement.slx differ diff --git a/demonstration/demonstration_main.m b/demonstration/demonstration_main.m new file mode 100644 index 0000000..49f9391 --- /dev/null +++ b/demonstration/demonstration_main.m @@ -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 \ No newline at end of file diff --git a/init_data_demonstration.m b/demonstration/displacement_init.m similarity index 100% rename from init_data_demonstration.m rename to demonstration/displacement_init.m diff --git a/demonstration/displacement_sim.m b/demonstration/displacement_sim.m new file mode 100644 index 0000000..3d332e3 --- /dev/null +++ b/demonstration/displacement_sim.m @@ -0,0 +1,5 @@ +%% +clear; close all; clc; + +%% +sim('Micro_Station_Displacement.slx'); diff --git a/demonstration/sample_pos_init.m b/demonstration/sample_pos_init.m new file mode 100644 index 0000000..54cb80d --- /dev/null +++ b/demonstration/sample_pos_init.m @@ -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)); diff --git a/demonstration/sample_pos_sim.m b/demonstration/sample_pos_sim.m new file mode 100644 index 0000000..6c36ede --- /dev/null +++ b/demonstration/sample_pos_sim.m @@ -0,0 +1,5 @@ +%% +clear; close all; clc; + +%% +sim('Micro_Station_Displacement.slx'); \ No newline at end of file diff --git a/hac_lac/hac_lac_iff_control.m b/hac_lac/hac_lac_iff_control.m new file mode 100644 index 0000000..e2c1173 --- /dev/null +++ b/hac_lac/hac_lac_iff_control.m @@ -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'); diff --git a/hac_lac/hac_lac_iff_control_plots.m b/hac_lac/hac_lac_iff_control_plots.m new file mode 100644 index 0000000..c803fba --- /dev/null +++ b/hac_lac/hac_lac_iff_control_plots.m @@ -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 \ No newline at end of file diff --git a/hac_lac/hac_lac_iff_simulation.m b/hac_lac/hac_lac_iff_simulation.m new file mode 100644 index 0000000..bd81675 --- /dev/null +++ b/hac_lac/hac_lac_iff_simulation.m @@ -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'); \ No newline at end of file diff --git a/hac_lac/hac_lac_main.m b/hac_lac/hac_lac_main.m new file mode 100644 index 0000000..3a24ffc --- /dev/null +++ b/hac_lac/hac_lac_main.m @@ -0,0 +1,2 @@ +%% +clear; close all; clc; diff --git a/identification/id_G_cart_plots.m b/identification/id_G_cart_plots.m new file mode 100644 index 0000000..4ba0c57 --- /dev/null +++ b/identification/id_G_cart_plots.m @@ -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 diff --git a/identification/id_G_d_plots.m b/identification/id_G_d_plots.m new file mode 100644 index 0000000..a2ff795 --- /dev/null +++ b/identification/id_G_d_plots.m @@ -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'); \ No newline at end of file diff --git a/identification/id_G_iff_plots.m b/identification/id_G_iff_plots.m new file mode 100644 index 0000000..aec98c6 --- /dev/null +++ b/identification/id_G_iff_plots.m @@ -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 diff --git a/identification/id_G_plots.m b/identification/id_G_plots.m deleted file mode 100644 index 3d15275..0000000 --- a/identification/id_G_plots.m +++ /dev/null @@ -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 diff --git a/identification/id_Gd.m b/identification/id_Gd.m deleted file mode 100644 index 67cfce0..0000000 --- a/identification/id_Gd.m +++ /dev/null @@ -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'); diff --git a/identification/id_Gd_plots.m b/identification/id_Gd_plots.m deleted file mode 100644 index 2855759..0000000 --- a/identification/id_Gd_plots.m +++ /dev/null @@ -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 diff --git a/identification/id_main.m b/identification/id_main.m index 1525bc1..fd73bea 100644 --- a/identification/id_main.m +++ b/identification/id_main.m @@ -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 diff --git a/identification/id_G.m b/identification/id_nano_station.m similarity index 52% rename from identification/id_G.m rename to identification/id_nano_station.m index 876464d..2e10216 100644 --- a/identification/id_G.m +++ b/identification/id_nano_station.m @@ -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'); diff --git a/identification/sim_nano_station.slx b/identification/sim_nano_station.slx index f2a100e..873053a 100644 Binary files a/identification/sim_nano_station.slx and b/identification/sim_nano_station.slx differ diff --git a/init_simulation.m b/init_simulation.m index 9f48487..3c174db 100644 --- a/init_simulation.m +++ b/init_simulation.m @@ -23,3 +23,4 @@ load('./mat/inputs.mat', 'inputs'); %% Load Controller load('./mat/controller.mat', 'K'); +load('./mat/K_iff.mat', 'K_iff'); diff --git a/initialize/initializeExperiment.m b/initialize/initializeExperiment.m new file mode 100644 index 0000000..88f47b3 --- /dev/null +++ b/initialize/initializeExperiment.m @@ -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 diff --git a/initialize/initializeInputs.m b/initialize/initializeInputs.m index 984438c..bd0df67 100644 --- a/initialize/initializeInputs.m +++ b/initialize/initializeInputs.m @@ -1,13 +1,13 @@ function [inputs] = initializeInputs(opts_param) %% Default values for opts - opts = struct('setpoint', false, ... + opts = struct('setpoint', false, ... 'ground_motion', false, ... - 'ty', false, ... - 'ry', false, ... - 'rz', false, ... % If numerical value, rpm speed of the spindle - 'u_hexa', false, ... - 'mass', false, ... - 'n_hexa', false ... + 'ty', false, ... + 'ry', false, ... + 'rz', false, ... % If numerical value, rpm speed of the spindle + 'u_hexa', false, ... + 'mass', false, ... + 'n_hexa', false ... ); %% Populate opts with input parameters @@ -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('./mat/inputs.mat', 'inputs'); - end + %% Save + save('./mat/inputs.mat', 'inputs'); end diff --git a/initialize/initializeSimConf.m b/initialize/initializeSimConf.m index e8352e4..9ca9655 100644 --- a/initialize/initializeSimConf.m +++ b/initialize/initializeSimConf.m @@ -1,9 +1,9 @@ function [] = initializeSimConf(opts_param) %% Default values for opts - 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] + 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 ); %% 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 - diff --git a/initialize/initializeSmiData.m b/initialize/initializeSmiData.m index 09ae573..beedc8e 100644 --- a/initialize/initializeSmiData.m +++ b/initialize/initializeSmiData.m @@ -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('./mat/smiData.mat', 'smiData') - end + %% Save + save('./mat/smiData.mat', 'smiData') end diff --git a/main.m b/main.m index 7e3fbea..db30065 100644 --- a/main.m +++ b/main.m @@ -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 \ No newline at end of file diff --git a/mat/G.mat b/mat/G.mat new file mode 100644 index 0000000..aa873cd Binary files /dev/null and b/mat/G.mat differ diff --git a/mat/G_f_to_d.mat b/mat/G_f_to_d.mat index 1ece31d..91eab7a 100644 Binary files a/mat/G_f_to_d.mat and b/mat/G_f_to_d.mat differ diff --git a/mat/G_iff.mat b/mat/G_iff.mat new file mode 100644 index 0000000..0e0299d Binary files /dev/null and b/mat/G_iff.mat differ diff --git a/mat/K_fb.mat b/mat/K_fb.mat new file mode 100644 index 0000000..f4e6db6 Binary files /dev/null and b/mat/K_fb.mat differ diff --git a/mat/K_fb_iff.mat b/mat/K_fb_iff.mat new file mode 100644 index 0000000..5372a41 Binary files /dev/null and b/mat/K_fb_iff.mat differ diff --git a/mat/K_iff.mat b/mat/K_iff.mat new file mode 100644 index 0000000..995e31c Binary files /dev/null and b/mat/K_iff.mat differ diff --git a/mat/K_iff_crit.mat b/mat/K_iff_crit.mat new file mode 100644 index 0000000..9ea5751 Binary files /dev/null and b/mat/K_iff_crit.mat differ diff --git a/mat/axisc.mat b/mat/axisc.mat index e4b72af..b4912f4 100644 Binary files a/mat/axisc.mat and b/mat/axisc.mat differ diff --git a/mat/config.mat b/mat/config.mat new file mode 100644 index 0000000..3eaada7 Binary files /dev/null and b/mat/config.mat differ diff --git a/mat/controller.mat b/mat/controller.mat index 69ff00d..6fbb4e3 100644 Binary files a/mat/controller.mat and b/mat/controller.mat differ diff --git a/mat/granite.mat b/mat/granite.mat index dc1e8e6..9b1375c 100644 Binary files a/mat/granite.mat and b/mat/granite.mat differ diff --git a/mat/ground.mat b/mat/ground.mat index 97814cd..7cb7ad6 100644 Binary files a/mat/ground.mat and b/mat/ground.mat differ diff --git a/mat/id_micro_station.mat b/mat/id_micro_station.mat new file mode 100644 index 0000000..79321b1 Binary files /dev/null and b/mat/id_micro_station.mat differ diff --git a/mat/inputs.mat b/mat/inputs.mat index a62a3cd..69a6bd6 100644 Binary files a/mat/inputs.mat and b/mat/inputs.mat differ diff --git a/mat/micro_hexapod.mat b/mat/micro_hexapod.mat index 458ce75..29c68ac 100644 Binary files a/mat/micro_hexapod.mat and b/mat/micro_hexapod.mat differ diff --git a/mat/mirror.mat b/mat/mirror.mat index 643192a..84a2ca6 100644 Binary files a/mat/mirror.mat and b/mat/mirror.mat differ diff --git a/mat/nano_hexapod.mat b/mat/nano_hexapod.mat index 96f55d4..4904655 100644 Binary files a/mat/nano_hexapod.mat and b/mat/nano_hexapod.mat differ diff --git a/mat/ry.mat b/mat/ry.mat index ad333be..5f45773 100644 Binary files a/mat/ry.mat and b/mat/ry.mat differ diff --git a/mat/rz.mat b/mat/rz.mat index 603f5fb..fc5eccf 100644 Binary files a/mat/rz.mat and b/mat/rz.mat differ diff --git a/mat/sample.mat b/mat/sample.mat index a4375d3..a92f02e 100644 Binary files a/mat/sample.mat and b/mat/sample.mat differ diff --git a/mat/sim_conf.mat b/mat/sim_conf.mat index 5d80932..d9c4623 100644 Binary files a/mat/sim_conf.mat and b/mat/sim_conf.mat differ diff --git a/mat/smiData.mat b/mat/smiData.mat index 3d32906..018a999 100644 Binary files a/mat/smiData.mat and b/mat/smiData.mat differ diff --git a/mat/stewart.mat b/mat/stewart.mat new file mode 100644 index 0000000..86bc947 Binary files /dev/null and b/mat/stewart.mat differ diff --git a/mat/ty.mat b/mat/ty.mat index 6dcb1ad..3da5102 100644 Binary files a/mat/ty.mat and b/mat/ty.mat differ diff --git a/src/computePsdDispl.m b/src/computePsdDispl.m new file mode 100644 index 0000000..0e41ca5 --- /dev/null +++ b/src/computePsdDispl.m @@ -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 diff --git a/src/generateDiagPidControl.m b/src/generateDiagPidControl.m new file mode 100644 index 0000000..ca9b824 --- /dev/null +++ b/src/generateDiagPidControl.m @@ -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 diff --git a/src/identifyG.m b/src/identifyG.m deleted file mode 100644 index 8e82b9f..0000000 --- a/src/identifyG.m +++ /dev/null @@ -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 diff --git a/src/identifyGd.m b/src/identifyGd.m deleted file mode 100644 index a7f5506..0000000 --- a/src/identifyGd.m +++ /dev/null @@ -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 diff --git a/src/identifyNass.m b/src/identifyNass.m deleted file mode 100644 index 7fbdb9f..0000000 --- a/src/identifyNass.m +++ /dev/null @@ -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 diff --git a/src/identifyPlant.m b/src/identifyPlant.m new file mode 100644 index 0000000..99614de --- /dev/null +++ b/src/identifyPlant.m @@ -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 + diff --git a/src/runSimulation.m b/src/runSimulation.m new file mode 100644 index 0000000..40d7466 --- /dev/null +++ b/src/runSimulation.m @@ -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 diff --git a/stewart-simscape b/stewart-simscape index bef54b5..9d12509 160000 --- a/stewart-simscape +++ b/stewart-simscape @@ -1 +1 @@ -Subproject commit bef54b5bf7967d8eb61a2d24d39a729e5d34c76d +Subproject commit 9d1250990137ddc246a1d7bf4084b36377cc55a3