[major changes] Add some control on the system

Add many scripts to:
- define all the inputs for various experiments
- plot the time and frequency data
- identify the plant

.gitignore

@@ -27,3 +27,4 @@ octave-workspace
 # Custom
 Assemblage_grt_rtw/
 Figures/
+data/

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')

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')

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')

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')

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('../mat/id_G_h_h.mat', 'G_h_h');
-load('../data/id_G_g_g.mat', 'G_g_g');
+load('../mat/id_G_g_g.mat', 'G_g_g');
-load('../data/id_G_h_g.mat', 'G_h_g');
+load('../mat/id_G_h_g.mat', 'G_h_g');
 
 %%
 freqs = logspace(-1, 3, 2000);

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

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('../mat/id_G_h_h.mat', 'G_h_h');
-save('../data/id_G_g_g.mat', 'G_g_g');
+save('../mat/id_G_g_g.mat', 'G_g_g');
-save('../data/id_G_h_g.mat', 'G_h_g');
+save('../mat/id_G_h_g.mat', 'G_h_g');

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;

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)

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

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