diff --git a/.gitignore b/.gitignore index 65018e3..1415e77 100644 --- a/.gitignore +++ b/.gitignore @@ -27,3 +27,4 @@ octave-workspace # Custom Assemblage_grt_rtw/ Figures/ +data/ diff --git a/Analysis/ground_motion_experiment.m b/Analysis/ground_motion_experiment.m new file mode 100644 index 0000000..1a43735 --- /dev/null +++ b/Analysis/ground_motion_experiment.m @@ -0,0 +1,71 @@ +%% Load open loop data +gm_ol = load('../data/ground_motion_001.mat'); + +%% +Dmeas.Data = Dmeas.Data - Dmeas.Data(1, :); + +%% Time domain X-Y-Z +figure; +hold on; +plot(Dmeas.Time, Dmeas.Data(:, 1)); +plot(Dmeas.Time, Dmeas.Data(:, 2)); +plot(Dmeas.Time, Dmeas.Data(:, 3)); +legend({'x', 'y', 'z'}) +hold off; +xlabel('Time [s]'); ylabel('Displacement [m]'); + +exportFig('tomo_time_translations', 'normal-normal') + +%% Time domain angles +figure; +hold on; +plot(Dmeas.Time, Dmeas.Data(:, 4)); +plot(Dmeas.Time, Dmeas.Data(:, 5)); +plot(Dmeas.Time, Dmeas.Data(:, 6)); +legend({'$\theta_x$', '$\theta_y$', '$\theta_z$'}) +hold off; +xlabel('Time [s]'); ylabel('Displacement [m]'); + +exportFig('tomo_time_rotations', 'normal-normal') + +%% PSD X-Y-Z +han_windows = hanning(ceil(length(Dmeas.Time)/10)); + +[psd_x, freqs_x] = pwelch(Dmeas.Data(:, 1), han_windows, 0, [], 1/Ts); +[psd_y, freqs_y] = pwelch(Dmeas.Data(:, 2), han_windows, 0, [], 1/Ts); +[psd_z, freqs_z] = pwelch(Dmeas.Data(:, 3), han_windows, 0, [], 1/Ts); + +figure; +hold on; +plot(freqs_x, sqrt(psd_x)); +plot(freqs_y, sqrt(psd_y)); +plot(freqs_z, sqrt(psd_z)); +set(gca,'xscale','log'); set(gca,'yscale','log'); +xlabel('Frequency [Hz]'); ylabel('PSD [$m/\sqrt{Hz}$]'); +legend({'x', 'y', 'z'}) +hold off; + +exportFig('tomo_psd_translations', 'normal-normal') + +%% PSD X-Y-Z +han_windows = hanning(ceil(length(Dmeas.Time)/10)); + +[psd_x, freqs_x] = pwelch(Dmeas.Data(:, 4), han_windows, 0, [], 1/Ts); +[psd_y, freqs_y] = pwelch(Dmeas.Data(:, 5), han_windows, 0, [], 1/Ts); +[psd_z, freqs_z] = pwelch(Dmeas.Data(:, 6), han_windows, 0, [], 1/Ts); + +figure; +hold on; +plot(freqs_x, sqrt(psd_x)); +plot(freqs_y, sqrt(psd_y)); +plot(freqs_z, sqrt(psd_z)); +set(gca,'xscale','log'); set(gca,'yscale','log'); +xlabel('Frequency [Hz]'); ylabel('PSD [$rad/s/\sqrt{Hz}$]'); +legend({'$\theta_x$', '$\theta_y$', '$\theta_z$'}) +hold off; + +exportFig('tomo_psd_rotations', 'normal-normal') + + +%% +save('../data/ground_motion.mat', 'Dmeas') \ No newline at end of file diff --git a/Analysis/ground_motion_ol_cl.m b/Analysis/ground_motion_ol_cl.m new file mode 100644 index 0000000..38bbec5 --- /dev/null +++ b/Analysis/ground_motion_ol_cl.m @@ -0,0 +1,33 @@ +gm_ol = load('../data/ground_motion_ol.mat', 'Dmeas'); +gm_cl = load('../data/ground_motion_001.mat', 'Dmeas'); + + +%% Compare OL and CL - Time +figure; +hold on; +plot(gm_ol.Dmeas.Time, gm_ol.Dmeas.Data(:, 2)); +plot(gm_cl.Dmeas.Time, gm_cl.Dmeas.Data(:, 2)); +legend({'y - OL', 'y - CL'}) +hold off; +xlabel('Time [s]'); ylabel('Displacement [m]'); + +exportFig('gm_control_time_y', 'normal-normal') + + +%% Compare OL and CL - PSD +han_windows_ol = hanning(ceil(length(gm_ol.Dmeas.Time)/10)); +[psd_y_ol, freqs_y_ol] = pwelch(gm_ol.Dmeas.Data(:, 2), han_windows, 0, [], 1/Ts); + +han_windows = hanning(ceil(length(gm_cl.Dmeas.Time)/10)); +[psd_y, freqs_y] = pwelch(gm_cl.Dmeas.Data(:, 2), han_windows, 0, [], 1/Ts); + +figure; +hold on; +plot(freqs_y_ol, sqrt(psd_y_ol)); +plot(freqs_y, sqrt(psd_y)); +set(gca,'xscale','log'); set(gca,'yscale','log'); +xlabel('Frequency [Hz]'); ylabel('PSD [$m/\sqrt{Hz}$]'); +legend({'y - OL', 'y - CL'}) +hold off; + +exportFig('gm_control_psd_y', 'normal-normal') diff --git a/Analysis/tomography_experiment.m b/Analysis/tomography_experiment.m new file mode 100644 index 0000000..010afeb --- /dev/null +++ b/Analysis/tomography_experiment.m @@ -0,0 +1,65 @@ +%% +Dmeas.Data = Dmeas.Data - Dmeas.Data(1, :); + +%% Time domain X-Y-Z +figure; +hold on; +plot(Dmeas.Time, Dmeas.Data(:, 1)); +plot(Dmeas.Time, Dmeas.Data(:, 2)); +plot(Dmeas.Time, Dmeas.Data(:, 3)); +legend({'x', 'y', 'z'}) +hold off; +xlabel('Time [s]'); ylabel('Displacement [m]'); + +exportFig('tomo_time_translations', 'normal-normal') + +%% Time domain angles +figure; +hold on; +plot(Dmeas.Time, Dmeas.Data(:, 4)); +plot(Dmeas.Time, Dmeas.Data(:, 5)); +legend({'$\theta_x$', '$\theta_y$'}) +hold off; +xlabel('Time [s]'); ylabel('Displacement [m]'); + +exportFig('tomo_time_rotations', 'normal-normal') + +%% PSD X-Y-Z +han_windows = hanning(ceil(length(Dmeas.Time)/10)); + +[psd_x, freqs_x] = pwelch(Dmeas.Data(:, 1), han_windows, 0, [], 1/Ts); +[psd_y, freqs_y] = pwelch(Dmeas.Data(:, 2), han_windows, 0, [], 1/Ts); +[psd_z, freqs_z] = pwelch(Dmeas.Data(:, 3), han_windows, 0, [], 1/Ts); + +figure; +hold on; +plot(freqs_x, sqrt(psd_x)); +plot(freqs_y, sqrt(psd_y)); +plot(freqs_z, sqrt(psd_z)); +set(gca,'xscale','log'); set(gca,'yscale','log'); +xlabel('Frequency [Hz]'); ylabel('PSD [$m/\sqrt{Hz}$]'); +legend({'x', 'y', 'z'}) +hold off; + +exportFig('tomo_psd_translations', 'normal-normal') + +%% PSD X-Y-Z +han_windows = hanning(ceil(length(Dmeas.Time)/10)); + +[psd_x, freqs_x] = pwelch(Dmeas.Data(:, 4), han_windows, 0, [], 1/Ts); +[psd_y, freqs_y] = pwelch(Dmeas.Data(:, 5), han_windows, 0, [], 1/Ts); + +figure; +hold on; +plot(freqs_x, sqrt(psd_x)); +plot(freqs_y, sqrt(psd_y)); +set(gca,'xscale','log'); set(gca,'yscale','log'); +xlabel('Frequency [Hz]'); ylabel('PSD [$rad/s/\sqrt{Hz}$]'); +legend({'$\theta_x$', '$\theta_y$'}) +hold off; + +exportFig('tomo_psd_rotations', 'normal-normal') + + +%% +save('../data/tomography_exp_ol.mat', 'Dmeas') \ No newline at end of file diff --git a/Analysis/tomography_ol_cl.m b/Analysis/tomography_ol_cl.m new file mode 100644 index 0000000..ad53667 --- /dev/null +++ b/Analysis/tomography_ol_cl.m @@ -0,0 +1,32 @@ +tomo_ol = load('../data/tomography_exp_ol.mat', 'Dmeas'); +tomo_cl = load('../data/tomography_exp_001.mat', 'Dmeas'); + +%% Compare OL and CL - Time +figure; +hold on; +plot(tomo_ol.Dmeas.Time, tomo_ol.Dmeas.Data(:, 2)); +plot(tomo_cl.Dmeas.Time, tomo_cl.Dmeas.Data(:, 2)); +legend({'y - OL', 'y - CL'}) +hold off; +xlabel('Time [s]'); ylabel('Displacement [m]'); + +exportFig('tomo_control_time_y', 'normal-normal') + + +%% Compare OL and CL - PSD +han_windows_ol = hanning(ceil(length(tomo_ol.Dmeas.Time)/10)); +[psd_y_ol, freqs_y_ol] = pwelch(tomo_ol.Dmeas.Data(:, 2), han_windows, 0, [], 1/Ts); + +han_windows = hanning(ceil(length(tomo_cl.Dmeas.Time)/10)); +[psd_y, freqs_y] = pwelch(tomo_cl.Dmeas.Data(:, 2), han_windows, 0, [], 1/Ts); + +figure; +hold on; +plot(freqs_y_ol, sqrt(psd_y_ol)); +plot(freqs_y, sqrt(psd_y)); +set(gca,'xscale','log'); set(gca,'yscale','log'); +xlabel('Frequency [Hz]'); ylabel('PSD [$m/\sqrt{Hz}$]'); +legend({'y - OL', 'y - CL'}) +hold off; + +exportFig('tomo_control_psd_y', 'normal-normal') diff --git a/Assemblage.slx b/Assemblage.slx index 3676e33..3e565a3 100644 Binary files a/Assemblage.slx and b/Assemblage.slx differ diff --git a/Control/control_y.m b/Control/control_y.m new file mode 100644 index 0000000..ae348d6 Binary files /dev/null and b/Control/control_y.m differ diff --git a/Identification/compare_measurements.m b/Identification/compare_measurements.m index dcd5232..1ee6e36 100644 --- a/Identification/compare_measurements.m +++ b/Identification/compare_measurements.m @@ -23,9 +23,9 @@ measure_dirs = {{'tx', 'tx'}, {'ty', 'ty'}, {'tz', 'tz'}}; measures = getAllMeasure(m_object, 'marble', 'hexa', measure_dirs, meas_opts); %% -load('../data/id_G_h_h.mat', 'G_h_h'); -load('../data/id_G_g_g.mat', 'G_g_g'); -load('../data/id_G_h_g.mat', 'G_h_g'); +load('../mat/id_G_h_h.mat', 'G_h_h'); +load('../mat/id_G_g_g.mat', 'G_g_g'); +load('../mat/id_G_h_g.mat', 'G_h_g'); %% freqs = logspace(-1, 3, 2000); diff --git a/Identification/identification_control.m b/Identification/identification_control.m new file mode 100644 index 0000000..e040445 --- /dev/null +++ b/Identification/identification_control.m @@ -0,0 +1,48 @@ +%% Script Description +% +%% +clear; +close all; +clc + +%% Define options for bode plots +bode_opts = bodeoptions; + +bode_opts.Title.FontSize = 12; +bode_opts.XLabel.FontSize = 12; +bode_opts.YLabel.FontSize = 12; +bode_opts.FreqUnits = 'Hz'; +bode_opts.MagUnits = 'abs'; +bode_opts.MagScale = 'log'; +bode_opts.PhaseWrapping = 'on'; +bode_opts.PhaseVisible = 'on'; + +%% Options for preprocessing the identified transfer functions +f_low = 10; +f_high = 1000; + +%% Options for Linearized +options = linearizeOptions; +options.SampleTime = 0; + +%% Name of the Simulink File +mdl = 'Assemblage'; + +%% Y-Translation Stage +% Input/Output definition +io(1) = linio([mdl, '/Fnass_cart'],1,'input'); +io(2) = linio([mdl, '/Sample'],1,'output'); + +% Run the linearization +G_f_to_d = linearize(mdl,io, 0); + +% Input/Output names +G_f_to_d.InputName = {'Fy'}; +G_f_to_d.OutputName = {'Dy'}; + +% Bode Plot of the linearized function +figure; +bode(G_f_to_d(2, 2), bode_opts) + +%% +save('../mat/G_f_to_d.mat', 'G_f_to_d'); diff --git a/Identification/identification_marc.m b/Identification/identification_marc.m index 9674f66..85c0338 100644 --- a/Identification/identification_marc.m +++ b/Identification/identification_marc.m @@ -80,6 +80,6 @@ figure; bode(G_h_g(2, 2), bode_opts) %% -save('../data/id_G_h_h.mat', 'G_h_h'); -save('../data/id_G_g_g.mat', 'G_g_g'); -save('../data/id_G_h_g.mat', 'G_h_g'); +save('../mat/id_G_h_h.mat', 'G_h_h'); +save('../mat/id_G_g_g.mat', 'G_g_g'); +save('../mat/id_G_h_g.mat', 'G_h_g'); diff --git a/Identification/identification_stages_plot.m b/Identification/identification_stages_plot.m index dc3002c..2d1993a 100644 --- a/Identification/identification_stages_plot.m +++ b/Identification/identification_stages_plot.m @@ -19,7 +19,7 @@ bode_opts.PhaseWrapping = 'on'; bode_opts.PhaseVisible = 'off'; %% Load Data -load('./data/identified_tf.mat'); +load('../mat/identified_tf.mat'); %% Y-Translation Stage figure; diff --git a/Identification/identification_stages_run.m b/Identification/identification_stages_run.m index 2e3ed55..7c8b33a 100644 --- a/Identification/identification_stages_run.m +++ b/Identification/identification_stages_run.m @@ -128,7 +128,7 @@ figure; bode(G_nass, bode_opts) %% Save all transfer function -save('./data/identified_tf.mat', 'G_ty', 'G_ry', 'G_rz', 'G_hexa', 'G_nass') +save('../mat/identified_tf.mat', 'G_ty', 'G_ry', 'G_rz', 'G_hexa', 'G_nass') %% Functions function G = preprocessIdTf(G0, f_low, f_high) diff --git a/init_data.m b/init_data.m index 778d640..bd3a330 100644 --- a/init_data.m +++ b/init_data.m @@ -1,11 +1,9 @@ -clear; close all; clc; - %% run init_solidworks_data.m %% Solver Configuration Ts = 1e-4; % Sampling time [s] -Tsim = 1; % Simulation time [s] +Tsim = 5; % Simulation time [s] %% Gravity g = 0 ; % Gravity along the z axis [m/s^2] @@ -63,7 +61,7 @@ rz.m = smiData.Solid(12).mass+6*smiData.Solid(20).mass+smiData.Solid(19).mass; rz.k.ax = 2e9; % Axial Stiffness [N/m] rz.k.rad = 7e8; % Radial Stiffness [N/m] rz.k.tilt = 1e5; % TODO -rz.k.rot = 1e5; % TODO +rz.k.rot = 1e5; % Rotational Stiffness [N*m/deg] rz.ksi.ax = 10; rz.ksi.rad = 10; @@ -100,6 +98,13 @@ sample.mass = 50; % Sample mass [kg] sample.offset = 0; % Decentralization offset [mm] sample.color = [0.9 0.1 0.1]; % Sample color +%% Signals Applied to the system +% load('./mat/inputs_ground_motion.mat'); +load('./mat/inputs_spindle.mat'); + +%% +load('./mat/controller.mat', 'K'); + %% function element = updateDamping(element) field = fieldnames(element.k); diff --git a/init_inputs.m b/init_inputs.m new file mode 100644 index 0000000..ade1ab7 --- /dev/null +++ b/init_inputs.m @@ -0,0 +1,42 @@ +%% +time_vector = 0:Ts:Tsim; + +%% Ground motion +r_Gm = timeseries(zeros(length(time_vector), 3), time_vector); + +% Wxg = 1e-5*(s/(2e2)^(1/3) + 2*pi*0.1)^3/(s + 2*pi*0.1)^3; +% Wxg = Wxg*(s/(0.5e6)^(1/3) + 2*pi*10)^3/(s + 2*pi*10)^3; +% Wxg = Wxg/(1+s/(2*pi*2000)); +% +% xg = 1/sqrt(2)*100*random('norm', 0, 1, length(time_vector), 3); +% xg(:, 1) = lsim(Wxg, xg(:, 1), time_vector); +% xg(:, 2) = lsim(Wxg, xg(:, 2), time_vector); +% xg(:, 3) = lsim(Wxg, xg(:, 3), time_vector); +% +% r_Gm = timeseries(xg, time_vector); +% +% figure; +% plot(r_Gm) + +%% Translation stage +r_Ty = timeseries(zeros(length(time_vector), 1), time_vector); + +%% Tilt Stage +r_My = timeseries(zeros(length(time_vector), 1), time_vector); + +%% Spindle +% r_Mz = timeseries(zeros(length(time_vector), 1), time_vector); + +r_Mz = timeseries(360*time_vector*rz.k.rot', time_vector); + +%% Micro Hexapod +r_u_hexa = timeseries(zeros(length(time_vector), 6), time_vector); + +%% Center of gravity compensation +r_mass = timeseries(zeros(length(time_vector), 2), time_vector); + +%% Nano Hexapod +r_n_hexa = timeseries(zeros(length(time_vector), 6), time_vector); + +%% +save('./mat/inputs_spindle.mat', 'r_Gm', 'r_Ty', 'r_My', 'r_u_hexa', 'r_mass', 'r_n_hexa'); \ No newline at end of file diff --git a/mat/G_f_to_d.mat b/mat/G_f_to_d.mat new file mode 100644 index 0000000..e37ac05 Binary files /dev/null and b/mat/G_f_to_d.mat differ diff --git a/mat/controller.mat b/mat/controller.mat new file mode 100644 index 0000000..2f69e6f Binary files /dev/null and b/mat/controller.mat differ diff --git a/mat/id_G_g_g.mat b/mat/id_G_g_g.mat new file mode 100644 index 0000000..a881982 Binary files /dev/null and b/mat/id_G_g_g.mat differ diff --git a/mat/id_G_h_g.mat b/mat/id_G_h_g.mat new file mode 100644 index 0000000..abcc0c7 Binary files /dev/null and b/mat/id_G_h_g.mat differ diff --git a/mat/id_G_h_h.mat b/mat/id_G_h_h.mat new file mode 100644 index 0000000..35c1477 Binary files /dev/null and b/mat/id_G_h_h.mat differ diff --git a/mat/identified_tf.mat b/mat/identified_tf.mat new file mode 100644 index 0000000..381fd96 Binary files /dev/null and b/mat/identified_tf.mat differ diff --git a/mat/inputs.mat b/mat/inputs.mat new file mode 100644 index 0000000..3de7cfa Binary files /dev/null and b/mat/inputs.mat differ diff --git a/mat/inputs_ground_motion.mat b/mat/inputs_ground_motion.mat new file mode 100644 index 0000000..212822b Binary files /dev/null and b/mat/inputs_ground_motion.mat differ diff --git a/mat/inputs_spindle.mat b/mat/inputs_spindle.mat new file mode 100644 index 0000000..da8bbbb Binary files /dev/null and b/mat/inputs_spindle.mat differ