add all files
This commit is contained in:
parent
3cc2105324
commit
37cd117a8f
1
.gitattributes
vendored
Normal file
1
.gitattributes
vendored
Normal file
@ -0,0 +1 @@
|
||||
*.mat filter=lfs diff=lfs merge=lfs -text
|
||||
BIN
A1-nass-uniaxial-model/mat/ground_motion_measurement.mat
(Stored with Git LFS)
Normal file
BIN
A1-nass-uniaxial-model/mat/ground_motion_measurement.mat
(Stored with Git LFS)
Normal file
Binary file not shown.
BIN
A1-nass-uniaxial-model/mat/meas_microstation_frf.mat
(Stored with Git LFS)
Normal file
BIN
A1-nass-uniaxial-model/mat/meas_microstation_frf.mat
(Stored with Git LFS)
Normal file
Binary file not shown.
BIN
A1-nass-uniaxial-model/mat/meas_spindle_off.mat
(Stored with Git LFS)
Normal file
BIN
A1-nass-uniaxial-model/mat/meas_spindle_off.mat
(Stored with Git LFS)
Normal file
Binary file not shown.
BIN
A1-nass-uniaxial-model/mat/meas_spindle_on.mat
(Stored with Git LFS)
Normal file
BIN
A1-nass-uniaxial-model/mat/meas_spindle_on.mat
(Stored with Git LFS)
Normal file
Binary file not shown.
BIN
A1-nass-uniaxial-model/mat/uniaxial_active_damping_controllers.mat
(Stored with Git LFS)
Normal file
BIN
A1-nass-uniaxial-model/mat/uniaxial_active_damping_controllers.mat
(Stored with Git LFS)
Normal file
Binary file not shown.
BIN
A1-nass-uniaxial-model/mat/uniaxial_damped_plants.mat
(Stored with Git LFS)
Normal file
BIN
A1-nass-uniaxial-model/mat/uniaxial_damped_plants.mat
(Stored with Git LFS)
Normal file
Binary file not shown.
BIN
A1-nass-uniaxial-model/mat/uniaxial_disturbance_psd.mat
(Stored with Git LFS)
Normal file
BIN
A1-nass-uniaxial-model/mat/uniaxial_disturbance_psd.mat
(Stored with Git LFS)
Normal file
Binary file not shown.
BIN
A1-nass-uniaxial-model/mat/uniaxial_high_authority_controllers.mat
(Stored with Git LFS)
Normal file
BIN
A1-nass-uniaxial-model/mat/uniaxial_high_authority_controllers.mat
(Stored with Git LFS)
Normal file
Binary file not shown.
BIN
A1-nass-uniaxial-model/mat/uniaxial_micro_station_parameters.mat
(Stored with Git LFS)
Normal file
BIN
A1-nass-uniaxial-model/mat/uniaxial_micro_station_parameters.mat
(Stored with Git LFS)
Normal file
Binary file not shown.
BIN
A1-nass-uniaxial-model/mat/uniaxial_plants.mat
(Stored with Git LFS)
Normal file
BIN
A1-nass-uniaxial-model/mat/uniaxial_plants.mat
(Stored with Git LFS)
Normal file
Binary file not shown.
BIN
A1-nass-uniaxial-model/nass_uniaxial_model.slx
Normal file
BIN
A1-nass-uniaxial-model/nass_uniaxial_model.slx
Normal file
Binary file not shown.
91
A1-nass-uniaxial-model/uniaxial_1_micro_station_model.m
Normal file
91
A1-nass-uniaxial-model/uniaxial_1_micro_station_model.m
Normal file
@ -0,0 +1,91 @@
|
||||
%% Clear Workspace and Close figures
|
||||
clear; close all; clc;
|
||||
|
||||
%% Intialize Laplace variable
|
||||
s = zpk('s');
|
||||
|
||||
%% Path for functions, data and scripts
|
||||
addpath('./mat/'); % Path for data
|
||||
|
||||
%% Colors for the figures
|
||||
colors = colororder;
|
||||
|
||||
%% Uniaxial Simscape model name
|
||||
mdl = 'nass_uniaxial_model';
|
||||
|
||||
%% Frequency Vector [Hz]
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
%% Load measured FRF
|
||||
load('meas_microstation_frf.mat');
|
||||
|
||||
%% Parameters - Mass
|
||||
mh = 15; % Micro Hexapod [kg]
|
||||
mt = 1200; % Ty + Ry + Rz [kg]
|
||||
mg = 2500; % Granite [kg]
|
||||
|
||||
%% Parameters - Stiffnesses
|
||||
kh = 6.11e+07; % [N/m]
|
||||
kt = 5.19e+08; % [N/m]
|
||||
kg = 9.50e+08; % [N/m]
|
||||
|
||||
%% Parameters - damping
|
||||
ch = 2*0.05*sqrt(kh*mh); % [N/(m/s)]
|
||||
ct = 2*0.05*sqrt(kt*mt); % [N/(m/s)]
|
||||
cg = 2*0.08*sqrt(kg*mg); % [N/(m/s)]
|
||||
|
||||
%% Save model parameters
|
||||
save('./mat/uniaxial_micro_station_parameters.mat', 'mh', 'mt', 'mg', 'ch', 'ct', 'cg', 'kh', 'kt', 'kg')
|
||||
|
||||
%% Disable the Nano-Hexpod for now
|
||||
model_config = struct();
|
||||
model_config.nhexa = "none";
|
||||
model_config.controller = "open_loop";
|
||||
|
||||
%% Identify the transfer function from u to taum
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/micro_station/Fg'], 1, 'openinput'); io_i = io_i + 1; % Hammer on Granite
|
||||
io(io_i) = linio([mdl, '/micro_station/Fh'], 1, 'openinput'); io_i = io_i + 1; % Hammer on Hexapod
|
||||
io(io_i) = linio([mdl, '/micro_station/xg'], 1, 'openoutput'); io_i = io_i + 1; % Absolute motion of Granite
|
||||
io(io_i) = linio([mdl, '/micro_station/xh'], 1, 'openoutput'); io_i = io_i + 1; % Absolute motion of Hexapod
|
||||
|
||||
%% Perform the model extraction
|
||||
G_id = linearize(mdl, io, 0.0);
|
||||
G_id.InputName = {'Fg', 'Fh'};
|
||||
G_id.OutputName = {'Dg', 'Dh'};
|
||||
|
||||
%% Comparison of the measured FRF and identified ones from the uniaxial model
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(f(f>20), abs(frf_Fhz_to_Dhz(f>20)), '-', 'color', colors(1,:), 'DisplayName', '$x_{h,z}/F_{h,z}$');
|
||||
plot(f(f>20), abs(frf_Fgz_to_Dhz(f>20)), '-', 'color', colors(2,:), 'DisplayName', '$x_{h,z}/F_{g,z}$');
|
||||
plot(f(f>20), abs(frf_Fgz_to_Dgz(f>20)), '-', 'color', colors(3,:), 'DisplayName', '$x_{g,z}/F_{g,z}$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_id('Dh', 'Fh'), freqs, 'Hz'))), '--', 'color', colors(1,:), 'DisplayName', '$x_{h,z}/F_{h,z}$ (model)');
|
||||
plot(freqs, abs(squeeze(freqresp(G_id('Dh', 'Fg'), freqs, 'Hz'))), '--', 'color', colors(2,:), 'DisplayName', '$x_{h,z}/F_{g,z}$ (model)');
|
||||
plot(freqs, abs(squeeze(freqresp(G_id('Dg', 'Fg'), freqs, 'Hz'))), '--', 'color', colors(3,:), 'DisplayName', '$x_{g,z}/F_{g,z}$ (model)');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
ylim([1e-10, 2e-7]);
|
||||
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 2);
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_id('Dh', 'Fh'), freqs, 'Hz')))), '--', 'color', colors(1,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_id('Dh', 'Fg'), freqs, 'Hz')))), '--', 'color', colors(2,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_id('Dg', 'Fg'), freqs, 'Hz')))), '--', 'color', colors(3,:));
|
||||
plot(f(f>20), 180/pi*unwrap(angle(frf_Fhx_to_Dhx(f>20))), '-', 'color', colors(1,:));
|
||||
plot(f(f>30), 180/pi*unwrap(angle(frf_Fgx_to_Dhx(f>30))), '-', 'color', colors(2,:));
|
||||
plot(f(f>20), 180/pi*unwrap(angle(frf_Fgx_to_Dgx(f>20))), '-', 'color', colors(3,:));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||||
hold off;
|
||||
yticks(-360:90:360);
|
||||
ylim([-360, 90]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([1, 500]);
|
||||
191
A1-nass-uniaxial-model/uniaxial_2_nano_hexapod_model.m
Normal file
191
A1-nass-uniaxial-model/uniaxial_2_nano_hexapod_model.m
Normal file
@ -0,0 +1,191 @@
|
||||
%% Clear Workspace and Close figures
|
||||
clear; close all; clc;
|
||||
|
||||
%% Intialize Laplace variable
|
||||
s = zpk('s');
|
||||
|
||||
%% Path for functions, data and scripts
|
||||
addpath('./mat/'); % Path for data
|
||||
|
||||
%% Colors for the figures
|
||||
colors = colororder;
|
||||
|
||||
%% Uniaxial Simscape model name
|
||||
mdl = 'nass_uniaxial_model';
|
||||
|
||||
%% Frequency Vector [Hz]
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
%% Load the micro-station parameters
|
||||
load('uniaxial_micro_station_parameters.mat')
|
||||
|
||||
%% Nano-Hexapod Parameters
|
||||
mn = 15; % [kg]
|
||||
kn = 1e7; % [N/m]
|
||||
cn = 2*0.01*sqrt(mn*kn); % [N/(m/s)]
|
||||
|
||||
%% Sample Mass
|
||||
ms = 10; % [kg]
|
||||
|
||||
%% Use 1DoF Nano-Hexpod model
|
||||
model_config = struct();
|
||||
model_config.nhexa = "1dof";
|
||||
model_config.controller = "open_loop";
|
||||
|
||||
%% Identify the transfer function from disturbances and force actuator to d
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/controller'], 1, 'openinput'); io_i = io_i + 1; % Force Actuator
|
||||
io(io_i) = linio([mdl, '/fs'], 1, 'openinput'); io_i = io_i + 1; % Force applied on the sample
|
||||
io(io_i) = linio([mdl, '/micro_station/xf'], 1, 'openinput'); io_i = io_i + 1; % Floor Motion
|
||||
io(io_i) = linio([mdl, '/micro_station/ft'], 1, 'openinput'); io_i = io_i + 1; % Stage disturbances
|
||||
io(io_i) = linio([mdl, '/d'] , 1, 'openoutput'); io_i = io_i + 1; % Metrology
|
||||
|
||||
%% Perform the model extraction
|
||||
G_ol = linearize(mdl, io, 0.0);
|
||||
G_ol.InputName = {'f', 'fs', 'xf', 'ft'};
|
||||
G_ol.OutputName = {'d'};
|
||||
|
||||
%% Sensitivity to disturbances - Fs
|
||||
figure;
|
||||
plot(freqs, abs(squeeze(freqresp(G_ol('d', 'fs'), freqs, 'Hz'))));
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude $d/f_{s}$ [m/N]'); xlabel('Frequency [Hz]');
|
||||
xticks([1e0, 1e1, 1e2]);
|
||||
xlim([1, 500]);
|
||||
|
||||
%% Sensitivity to disturbances - Ft
|
||||
figure;
|
||||
plot(freqs, abs(squeeze(freqresp(G_ol('d', 'ft'), freqs, 'Hz'))));
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude $d/f_{t}$ [m/N]'); xlabel('Frequency [Hz]');
|
||||
xticks([1e0, 1e1, 1e2]);
|
||||
xlim([1, 500]);
|
||||
|
||||
%% Sensitivity to disturbances - xf
|
||||
figure;
|
||||
plot(freqs, abs(squeeze(freqresp(G_ol('d', 'xf'), freqs, 'Hz'))));
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude $d/x_{f}$ [m/m]'); xlabel('Frequency [Hz]');
|
||||
xticks([1e0, 1e1, 1e2]);
|
||||
xlim([1, 500]);
|
||||
ylim([1e-2, 1e2]);
|
||||
|
||||
%% Bode Plot of the transfer function from actuator forces to measured displacement by the metrology
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_ol('d', 'f'), freqs, 'Hz'))));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude $d/f$ [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ol('d', 'f'), freqs, 'Hz'))));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||||
hold off;
|
||||
yticks(-360:90:360);
|
||||
ylim([-180, 0]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([1, 500]);
|
||||
|
||||
%% Use 1DoF Nano-Hexpod model
|
||||
model_config = struct();
|
||||
model_config.nhexa = "1dof";
|
||||
model_config.controller = "open_loop";
|
||||
|
||||
%% Nano-Hexapod Mass
|
||||
mn = 15; % Nano-Hexapod mass [kg]
|
||||
|
||||
%% Identification of all combination of stiffnesses / masses
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force
|
||||
io(io_i) = linio([mdl, '/micro_station/xf'], 1, 'openinput'); io_i = io_i + 1; % Floor Motion
|
||||
io(io_i) = linio([mdl, '/micro_station/ft'], 1, 'openinput'); io_i = io_i + 1; % Stage vibrations
|
||||
io(io_i) = linio([mdl, '/fs'], 1, 'openinput'); io_i = io_i + 1; % Direct sample forces
|
||||
io(io_i) = linio([mdl, '/dL'], 1, 'openoutput'); io_i = io_i + 1; % Relative Motion Sensor
|
||||
io(io_i) = linio([mdl, '/fm'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensor
|
||||
io(io_i) = linio([mdl, '/vn'] , 1, 'openoutput'); io_i = io_i + 1; % Geophone
|
||||
io(io_i) = linio([mdl, '/d'] , 1, 'openoutput'); io_i = io_i + 1; % Metrology Output
|
||||
|
||||
%% Light Sample
|
||||
ms = 1; % Sample Mass [kg]
|
||||
|
||||
% Voice Coil (i.e. soft) Nano-Hexapod
|
||||
kn = 1e4; % Nano-Hexapod Stiffness [N/m]
|
||||
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
|
||||
G_vc_light = linearize(mdl, io, 0.0);
|
||||
G_vc_light.InputName = {'f', 'xf', 'ft', 'fs'};
|
||||
G_vc_light.OutputName = {'dL', 'fm', 'vn', 'd'};
|
||||
|
||||
% APA (i.e. relatively stiff) Nano-Hexapod
|
||||
kn = 1e6; % Nano-Hexapod Stiffness [N/m]
|
||||
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
|
||||
G_md_light = linearize(mdl, io, 0.0);
|
||||
G_md_light.InputName = {'f', 'xf', 'ft', 'fs'};
|
||||
G_md_light.OutputName = {'dL', 'fm', 'vn', 'd'};
|
||||
|
||||
% Piezoelectric (i.e. stiff) Nano-Hexapod
|
||||
kn = 1e8; % Nano-Hexapod Stiffness [N/m]
|
||||
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
|
||||
G_pz_light = linearize(mdl, io, 0.0);
|
||||
G_pz_light.InputName = {'f', 'xf', 'ft', 'fs'};
|
||||
G_pz_light.OutputName = {'dL', 'fm', 'vn', 'd'};
|
||||
|
||||
%% Mid Sample
|
||||
ms = 25; % Sample Mass [kg]
|
||||
|
||||
% Voice Coil (i.e. soft) Nano-Hexapod
|
||||
kn = 1e4; % Nano-Hexapod Stiffness [N/m]
|
||||
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
|
||||
G_vc_mid = linearize(mdl, io, 0.0);
|
||||
G_vc_mid.InputName = {'f', 'xf', 'ft', 'fs'};
|
||||
G_vc_mid.OutputName = {'dL', 'fm', 'vn', 'd'};
|
||||
|
||||
% APA (i.e. relatively stiff) Nano-Hexapod
|
||||
kn = 1e6; % Nano-Hexapod Stiffness [N/m]
|
||||
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
|
||||
G_md_mid = linearize(mdl, io, 0.0);
|
||||
G_md_mid.InputName = {'f', 'xf', 'ft', 'fs'};
|
||||
G_md_mid.OutputName = {'dL', 'fm', 'vn', 'd'};
|
||||
|
||||
% Piezoelectric (i.e. stiff) Nano-Hexapod
|
||||
kn = 1e8; % Nano-Hexapod Stiffness [N/m]
|
||||
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
|
||||
G_pz_mid = linearize(mdl, io, 0.0);
|
||||
G_pz_mid.InputName = {'f', 'xf', 'ft', 'fs'};
|
||||
G_pz_mid.OutputName = {'dL', 'fm', 'vn', 'd'};
|
||||
|
||||
%% Heavy Sample
|
||||
ms = 50; % Sample Mass [kg]
|
||||
|
||||
% Voice Coil (i.e. soft) Nano-Hexapod
|
||||
kn = 1e4; % Nano-Hexapod Stiffness [N/m]
|
||||
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
|
||||
G_vc_heavy = linearize(mdl, io, 0.0);
|
||||
G_vc_heavy.InputName = {'f', 'xf', 'ft', 'fs'};
|
||||
G_vc_heavy.OutputName = {'dL', 'fm', 'vn', 'd'};
|
||||
|
||||
% APA (i.e. relatively stiff) Nano-Hexapod
|
||||
kn = 1e6; % Nano-Hexapod Stiffness [N/m]
|
||||
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
|
||||
G_md_heavy = linearize(mdl, io, 0.0);
|
||||
G_md_heavy.InputName = {'f', 'xf', 'ft', 'fs'};
|
||||
G_md_heavy.OutputName = {'dL', 'fm', 'vn', 'd'};
|
||||
|
||||
% Piezoelectric (i.e. stiff) Nano-Hexapod
|
||||
kn = 1e8; % Nano-Hexapod Stiffness [N/m]
|
||||
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
|
||||
G_pz_heavy = linearize(mdl, io, 0.0);
|
||||
G_pz_heavy.InputName = {'f', 'xf', 'ft', 'fs'};
|
||||
G_pz_heavy.OutputName = {'dL', 'fm', 'vn', 'd'};
|
||||
|
||||
%% Save All Identified Plants
|
||||
save('./mat/uniaxial_plants.mat', 'G_vc_light', 'G_md_light', 'G_pz_light', ...
|
||||
'G_vc_mid', 'G_md_mid', 'G_pz_mid', ...
|
||||
'G_vc_heavy', 'G_md_heavy', 'G_pz_heavy');
|
||||
115
A1-nass-uniaxial-model/uniaxial_3_disturbances.m
Normal file
115
A1-nass-uniaxial-model/uniaxial_3_disturbances.m
Normal file
@ -0,0 +1,115 @@
|
||||
%% Clear Workspace and Close figures
|
||||
clear; close all; clc;
|
||||
|
||||
%% Intialize Laplace variable
|
||||
s = zpk('s');
|
||||
|
||||
%% Path for functions, data and scripts
|
||||
addpath('./mat/'); % Path for data
|
||||
|
||||
%% Colors for the figures
|
||||
colors = colororder;
|
||||
|
||||
%% Uniaxial Simscape model name
|
||||
mdl = 'nass_uniaxial_model';
|
||||
|
||||
%% Frequency Vector [Hz]
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
%% Load the micro-station parameters
|
||||
load('uniaxial_micro_station_parameters.mat');
|
||||
|
||||
%% Compute Floor Motion Spectral Density
|
||||
% Load floor motion data
|
||||
% t: time in [s]
|
||||
% V: measured voltage genrated by the geophone and amplified by a 60dB gain voltage amplifier [V]
|
||||
load('ground_motion_measurement.mat', 't', 'V');
|
||||
|
||||
% Geophone Transfer Function
|
||||
Tg = 88; % Sensitivity [V/(m/s)]
|
||||
w0 = 2*2*pi; % Cut-off frequency [rad/s]
|
||||
xi = 0.7; % Damping ratio
|
||||
|
||||
G_geo = Tg*s*s^2/(s^2 + 2*xi*w0*s + w0^2); % Geophone's transfer function [V/m]
|
||||
|
||||
% Voltage amplifier transfer function
|
||||
g0 = 10^(60/20); % [abs]
|
||||
|
||||
% Compute measured voltage PSD
|
||||
Ts = (t(2)-t(1)); % Sampling Time [s]
|
||||
Nfft = floor(2/Ts);
|
||||
win = hanning(Nfft);
|
||||
Noverlap = floor(Nfft/2);
|
||||
|
||||
[psd_V, f] = pwelch(V, win, Noverlap, Nfft, 1/Ts); % [V^2/Hz]
|
||||
|
||||
% Ground Motion ASD
|
||||
psd_xf = psd_V./abs(squeeze(freqresp(G_geo*g0, f, 'Hz'))).^2; % [m^2/Hz]
|
||||
|
||||
%% Amplitude Spectral Density of the measured Floor motion on ID31
|
||||
figure;
|
||||
plot(f, sqrt(psd_xf), 'DisplayName', '$\Gamma_{x_{f}}$');
|
||||
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Ampl. Spectral Density $\left[\frac{m}{\sqrt{Hz}}\right]$')
|
||||
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
||||
xlim([1, 500]);
|
||||
xticks([1e0, 1e1, 1e2]);
|
||||
|
||||
%% Estimation of the Spectral density of the stage vibrations
|
||||
|
||||
% Measured velocity of granite and hexapod during spindle rotation
|
||||
% t: time in [s]
|
||||
% vg: measured granite velocity [m/s]
|
||||
% vg: measured micro-hexapod's top platform velocity [m/s]
|
||||
load('meas_spindle_on.mat', 't', 'vg', 'vh');
|
||||
spindle_off = load('meas_spindle_off.mat', 't', 'vg', 'vh'); % No Rotation
|
||||
|
||||
% Compute Power Spectral Density of the relative velocity between granite and hexapod during spindle rotation
|
||||
Fs = 1/(t(2)-t(1)); % Sampling Frequency [Hz]
|
||||
win = hanning(ceil(2*Fs)); % Hanning window
|
||||
|
||||
[psd_vft, f] = pwelch(vh-vg, win, [], [], Fs); % [(m/s)^2/Hz]
|
||||
[psd_off, ~] = pwelch(spindle_off.vh-spindle_off.vg, win, [], [], Fs); % [(m/s)^2/Hz]
|
||||
|
||||
% Disable the Nano-Hexpod for now
|
||||
model_config = struct();
|
||||
model_config.nhexa = "none";
|
||||
model_config.controller = "open_loop";
|
||||
|
||||
% Identify the transfer function from u to taum
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/micro_station/ft'], 1, 'openinput'); io_i = io_i + 1; % Stage Disturbance Force
|
||||
io(io_i) = linio([mdl, '/micro_station/xg'], 1, 'openoutput'); io_i = io_i + 1; % Absolute motion of Granite
|
||||
io(io_i) = linio([mdl, '/micro_station/xh'], 1, 'openoutput'); io_i = io_i + 1; % Absolute motion of Hexapod
|
||||
|
||||
% Perform the model extraction
|
||||
G = linearize(mdl, io, 0.0);
|
||||
G.InputName = {'ft'};
|
||||
G.OutputName = {'Dg', 'Dh'};
|
||||
|
||||
% Power Spectral Density of the equivalent force ft [N/Hz^2]
|
||||
psd_ft = (psd_vft./(2*pi*f).^2)./abs(squeeze(freqresp(G('Dh', 'ft') - G('Dg', 'ft'), f, 'Hz'))).^2;
|
||||
|
||||
%% Amplitude Spectral Density of the relative motion measured between the granite and the micro-hexapod's top platform during Spindle rotating
|
||||
figure;
|
||||
hold on;
|
||||
plot(f, sqrt(psd_vft)./(2*pi*f), 'DisplayName', '6rpm');
|
||||
plot(f, sqrt(psd_off)./(2*pi*f), 'DisplayName', '0rpm');
|
||||
hold off;
|
||||
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Ampl. Spectral Density $\left[\frac{m}{\sqrt{Hz}}\right]$')
|
||||
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
||||
xlim([1, 500]); ylim([1e-12, 1e-7])
|
||||
|
||||
%% Estimated disturbance force ft from measurement and uniaxial model
|
||||
figure;
|
||||
hold on;
|
||||
plot(f, sqrt(psd_ft), 'DisplayName', '$\Gamma_{f_{t}}$');
|
||||
hold off;
|
||||
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Ampl. Spectral Density $\left[\frac{N}{\sqrt{Hz}}\right]$')
|
||||
xlim([1, 500]);
|
||||
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
||||
|
||||
%% Save PSD of disturbances
|
||||
save('./mat/uniaxial_disturbance_psd.mat', 'f', 'psd_ft', 'psd_xf');
|
||||
110
A1-nass-uniaxial-model/uniaxial_4_dynamic_noise_budget.m
Normal file
110
A1-nass-uniaxial-model/uniaxial_4_dynamic_noise_budget.m
Normal file
@ -0,0 +1,110 @@
|
||||
%% Clear Workspace and Close figures
|
||||
clear; close all; clc;
|
||||
|
||||
%% Intialize Laplace variable
|
||||
s = zpk('s');
|
||||
|
||||
%% Path for functions, data and scripts
|
||||
addpath('./mat/'); % Path for data
|
||||
|
||||
%% Colors for the figures
|
||||
colors = colororder;
|
||||
|
||||
%% Frequency Vector [Hz]
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
%% Load the PSD of disturbances
|
||||
load('uniaxial_disturbance_psd.mat', 'f', 'psd_ft', 'psd_xf');
|
||||
|
||||
%% Load Plants Dynamics
|
||||
load('uniaxial_plants.mat', 'G_vc_light', 'G_md_light', 'G_pz_light', ...
|
||||
'G_vc_mid', 'G_md_mid', 'G_pz_mid', ...
|
||||
'G_vc_heavy', 'G_md_heavy', 'G_pz_heavy');
|
||||
|
||||
%% Sensitivity to disturbances for three different nano-hexpod stiffnesses
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_light('d', 'fs'), freqs, 'Hz'))));
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_light('d', 'fs'), freqs, 'Hz'))));
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_light('d', 'fs'), freqs, 'Hz'))));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude $d/f_{s}$ [m/N]'); xlabel('Frequency [Hz]');
|
||||
xticks([1e0, 1e1, 1e2]);
|
||||
xlim([1, 500]);
|
||||
|
||||
%% Sensitivity to disturbances for three different nano-hexpod stiffnesses
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_light('d', 'ft'), freqs, 'Hz'))));
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_light('d', 'ft'), freqs, 'Hz'))));
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_light('d', 'ft'), freqs, 'Hz'))));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude $d/f_{t}$ [m/N]'); xlabel('Frequency [Hz]');
|
||||
xticks([1e0, 1e1, 1e2]);
|
||||
xlim([1, 500]);
|
||||
|
||||
%% Sensitivity to disturbances for three different nano-hexpod stiffnesses
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_light('d', 'xf'), freqs, 'Hz'))), 'DisplayName', '$k_n = 0.01\,N/\mu m$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_light('d', 'xf'), freqs, 'Hz'))), 'DisplayName', '$k_n = 1 \,N/\mu m$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_light('d', 'xf'), freqs, 'Hz'))), 'DisplayName', '$k_n = 100 \,N/\mu m$');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude $d/x_{f}$ [m/m]'); xlabel('Frequency [Hz]');
|
||||
xticks([1e0, 1e1, 1e2]);
|
||||
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
||||
leg.ItemTokenSize(1) = 15
|
||||
xlim([1, 500]);
|
||||
|
||||
%% Cumulative Amplitude Spectrum of the relative motion d, due to both the floor motion and the stage vibrations
|
||||
figure;
|
||||
hold on;
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_vc_light('d', 'ft'), f, 'Hz'))).^2)))), '-', 'color', colors(1,:), 'DisplayName', '$f_t$');
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_md_light('d', 'ft'), f, 'Hz'))).^2)))), '-', 'color', colors(2,:), 'DisplayName', '$f_t$');
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_pz_light('d', 'ft'), f, 'Hz'))).^2)))), '-', 'color', colors(3,:), 'DisplayName', '$f_t$');
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_xf.*abs(squeeze(freqresp(G_vc_light('d', 'xf'), f, 'Hz'))).^2)))), '--', 'color', colors(1,:), 'DisplayName', '$x_f$ ($k_n = 0.01\,N/\mu m$)');
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_xf.*abs(squeeze(freqresp(G_md_light('d', 'xf'), f, 'Hz'))).^2)))), '--', 'color', colors(2,:), 'DisplayName', '$x_f$ ($k_n = 1 \,N/\mu m$)');
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_xf.*abs(squeeze(freqresp(G_pz_light('d', 'xf'), f, 'Hz'))).^2)))), '--', 'color', colors(3,:), 'DisplayName', '$x_f$ ($k_n = 100 \,N/\mu m$)');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('CAS [m]'); xlabel('Frequency [Hz]');
|
||||
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
|
||||
leg.ItemTokenSize(1) = 15
|
||||
|
||||
xlim([1, 500]);
|
||||
ylim([1e-12, 3e-6])
|
||||
|
||||
%% Cumulative Amplitude Spectrum of the relative motion d due to all disturbances, for two sample masses
|
||||
figure;
|
||||
hold on;
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_vc_light('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_vc_light('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'color', colors(1,:), 'DisplayName', '$m_s = 1\,kg$, $k_n = 0.01\,N/\mu m$');
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_md_light('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_md_light('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'color', colors(2,:), 'DisplayName', '$m_s = 1\,kg$, $k_n = 1\,N/\mu m$');
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_pz_light('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_pz_light('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'color', colors(3,:), 'DisplayName', '$m_s = 1\,kg$, $k_n = 100\,N/\mu m$');
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_vc_heavy('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_vc_heavy('d', 'xf'), f, 'Hz'))).^2)))), '--', ...
|
||||
'color', colors(1,:), 'DisplayName', '$m_s = 50\,kg$, $k_n = 0.01\,N/\mu m$');
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_md_heavy('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_md_heavy('d', 'xf'), f, 'Hz'))).^2)))), '--', ...
|
||||
'color', colors(2,:), 'DisplayName', '$m_s = 50\,kg$, $k_n = 1\,N/\mu m$');
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_pz_heavy('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_pz_heavy('d', 'xf'), f, 'Hz'))).^2)))), '--', ...
|
||||
'color', colors(3,:), 'DisplayName', '$m_s = 50\,kg$, $k_n = 100\,N/\mu m$');
|
||||
plot([1, 1e3], [20e-9, 20e-9], 'k--', 'HandleVisibility', 'off');
|
||||
text(4, 1e-8, '20 nm RMS', 'horizontalalignment', 'center');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('CAS [m]'); xlabel('Frequency [Hz]');
|
||||
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
||||
leg.ItemTokenSize(1) = 15
|
||||
|
||||
xlim([1, 500]);
|
||||
ylim([1e-12, 3e-6])
|
||||
630
A1-nass-uniaxial-model/uniaxial_5_active_damping.m
Normal file
630
A1-nass-uniaxial-model/uniaxial_5_active_damping.m
Normal file
@ -0,0 +1,630 @@
|
||||
%% Clear Workspace and Close figures
|
||||
clear; close all; clc;
|
||||
|
||||
%% Intialize Laplace variable
|
||||
s = zpk('s');
|
||||
|
||||
%% Path for functions, data and scripts
|
||||
addpath('./mat/'); % Path for data
|
||||
|
||||
%% Colors for the figures
|
||||
colors = colororder;
|
||||
|
||||
%% Frequency Vector [Hz]
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
%% Load the PSD of disturbances
|
||||
load('uniaxial_disturbance_psd.mat', 'f', 'psd_ft', 'psd_xf');
|
||||
|
||||
%% Load Plants Dynamics
|
||||
load('uniaxial_plants.mat', 'G_vc_light', 'G_md_light', 'G_pz_light', ...
|
||||
'G_vc_mid', 'G_md_mid', 'G_pz_mid', ...
|
||||
'G_vc_heavy', 'G_md_heavy', 'G_pz_heavy');
|
||||
|
||||
%% Damped plants for three considered payload masses - Comparison of active damping techniques
|
||||
% Integral Force Feedback
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_light('fm', 'f'), freqs, 'Hz'))), '-', 'color', colors(1,:), 'DisplayName', '$m_s = 1\,kg$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_mid( 'fm', 'f'), freqs, 'Hz'))), '-.', 'color', colors(1,:), 'DisplayName', '$m_s = 25\,kg$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_heavy('fm', 'f'), freqs, 'Hz'))), '--', 'color', colors(1,:), 'DisplayName', '$m_s = 50\,kg$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_light('fm', 'f'), freqs, 'Hz'))), '-', 'color', colors(2,:), 'HandleVisibility', 'off');
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_mid( 'fm', 'f'), freqs, 'Hz'))), '-.', 'color', colors(2,:), 'HandleVisibility', 'off');
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_heavy('fm', 'f'), freqs, 'Hz'))), '--', 'color', colors(2,:), 'HandleVisibility', 'off');
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_light('fm', 'f'), freqs, 'Hz'))), '-', 'color', colors(3,:), 'HandleVisibility', 'off');
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_mid( 'fm', 'f'), freqs, 'Hz'))), '-.', 'color', colors(3,:), 'HandleVisibility', 'off');
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_heavy('fm', 'f'), freqs, 'Hz'))), '--', 'color', colors(3,:), 'HandleVisibility', 'off');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
|
||||
ldg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
||||
ldg.ItemTokenSize = [20, 1];
|
||||
|
||||
ax1b = nexttile();
|
||||
hold on;
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_light('fm', 'f'), freqs, 'Hz')))), '-', 'color', colors(1,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_mid( 'fm', 'f'), freqs, 'Hz')))), '-.', 'color', colors(1,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_heavy('fm', 'f'), freqs, 'Hz')))), '--', 'color', colors(1,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_md_light('fm', 'f'), freqs, 'Hz')))), '-', 'color', colors(2,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_md_mid( 'fm', 'f'), freqs, 'Hz')))), '-.', 'color', colors(2,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_md_heavy('fm', 'f'), freqs, 'Hz')))), '--', 'color', colors(2,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_light('fm', 'f'), freqs, 'Hz')))), '-', 'color', colors(3,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_mid( 'fm', 'f'), freqs, 'Hz')))), '-.', 'color', colors(3,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_heavy('fm', 'f'), freqs, 'Hz')))), '--', 'color', colors(3,:));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||||
xticks([1e0, 1e1, 1e2]);
|
||||
yticks(-360:90:360);
|
||||
ylim([-200, 20]);
|
||||
|
||||
linkaxes([ax1,ax1b],'x');
|
||||
xlim([1, 1000]);
|
||||
|
||||
% Relative Motion Control
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax2 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_light('dL', 'f'), freqs, 'Hz'))), '-', 'color', colors(1,:));
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_mid( 'dL', 'f'), freqs, 'Hz'))), '-.', 'color', colors(1,:));
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_heavy('dL', 'f'), freqs, 'Hz'))), '--', 'color', colors(1,:));
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_light('dL', 'f'), freqs, 'Hz'))), '-', 'color', colors(2,:));
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_mid( 'dL', 'f'), freqs, 'Hz'))), '-.', 'color', colors(2,:));
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_heavy('dL', 'f'), freqs, 'Hz'))), '--', 'color', colors(2,:));
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_light('dL', 'f'), freqs, 'Hz'))), '-', 'color', colors(3,:));
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_mid( 'dL', 'f'), freqs, 'Hz'))), '-.', 'color', colors(3,:));
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_heavy('dL', 'f'), freqs, 'Hz'))), '--', 'color', colors(3,:));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
|
||||
ax2b = nexttile();
|
||||
hold on;
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_light('dL', 'f'), freqs, 'Hz')))), '-', 'color', colors(1,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_mid( 'dL', 'f'), freqs, 'Hz')))), '-.', 'color', colors(1,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_heavy('dL', 'f'), freqs, 'Hz')))), '--', 'color', colors(1,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_md_light('dL', 'f'), freqs, 'Hz')))), '-', 'color', colors(2,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_md_mid( 'dL', 'f'), freqs, 'Hz')))), '-.', 'color', colors(2,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_md_heavy('dL', 'f'), freqs, 'Hz')))), '--', 'color', colors(2,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_light('dL', 'f'), freqs, 'Hz')))), '-', 'color', colors(3,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_mid( 'dL', 'f'), freqs, 'Hz')))), '-.', 'color', colors(3,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_heavy('dL', 'f'), freqs, 'Hz')))), '--', 'color', colors(3,:));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||||
xticks([1e0, 1e1, 1e2]);
|
||||
yticks(-360:90:360);
|
||||
ylim([-200, 20]);
|
||||
|
||||
linkaxes([ax2,ax2b],'x');
|
||||
xlim([1, 1000]);
|
||||
|
||||
% Direct Velocity Feedback
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax3 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_light('vn', 'f'), freqs, 'Hz'))), '-', 'color', colors(1,:), 'DisplayName', '$k_n = 0.01\,N/\mu m$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_mid( 'vn', 'f'), freqs, 'Hz'))), '-.', 'color', colors(1,:), 'HandleVisibility', 'off');
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_heavy('vn', 'f'), freqs, 'Hz'))), '--', 'color', colors(1,:), 'HandleVisibility', 'off');
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_light('vn', 'f'), freqs, 'Hz'))), '-', 'color', colors(2,:), 'DisplayName', '$k_n = 1\,N/\mu m$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_mid( 'vn', 'f'), freqs, 'Hz'))), '-.', 'color', colors(2,:), 'HandleVisibility', 'off');
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_heavy('vn', 'f'), freqs, 'Hz'))), '--', 'color', colors(2,:), 'HandleVisibility', 'off');
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_light('vn', 'f'), freqs, 'Hz'))), '-', 'color', colors(3,:), 'DisplayName', '$k_n = 100\,N/\mu m$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_mid( 'vn', 'f'), freqs, 'Hz'))), '-.', 'color', colors(3,:), 'HandleVisibility', 'off');
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_heavy('vn', 'f'), freqs, 'Hz'))), '--', 'color', colors(3,:), 'HandleVisibility', 'off');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/s/N]'); set(gca, 'XTickLabel',[]);
|
||||
ldg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
||||
ldg.ItemTokenSize = [20, 1];
|
||||
|
||||
ax3b = nexttile();
|
||||
hold on;
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_light('vn', 'f'), freqs, 'Hz')))), '-', 'color', colors(1,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_mid( 'vn', 'f'), freqs, 'Hz')))), '-.', 'color', colors(1,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_heavy('vn', 'f'), freqs, 'Hz')))), '--', 'color', colors(1,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_md_light('vn', 'f'), freqs, 'Hz')))), '-', 'color', colors(2,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_md_mid( 'vn', 'f'), freqs, 'Hz')))), '-.', 'color', colors(2,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_md_heavy('vn', 'f'), freqs, 'Hz')))), '--', 'color', colors(2,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_light('vn', 'f'), freqs, 'Hz')))), '-', 'color', colors(3,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_mid( 'vn', 'f'), freqs, 'Hz')))), '-.', 'color', colors(3,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_heavy('vn', 'f'), freqs, 'Hz')))), '--', 'color', colors(3,:));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||||
xticks([1e0, 1e1, 1e2]);
|
||||
yticks(-360:90:360);
|
||||
ylim([-110, 110]);
|
||||
|
||||
linkaxes([ax3,ax3b],'x');
|
||||
xlim([1, 1000]);
|
||||
|
||||
%% Design of Active Damping controllers to have reasonable damping
|
||||
% IFF
|
||||
K_iff_vc = 20/(s + 2*pi*0.01);
|
||||
K_iff_vc.InputName = {'fm'};
|
||||
K_iff_vc.OutputName = {'f'};
|
||||
K_iff_md = 200/(s + 2*pi*0.01);
|
||||
K_iff_md.InputName = {'fm'};
|
||||
K_iff_md.OutputName = {'f'};
|
||||
K_iff_pz = 4000/(s + 2*pi*0.01);
|
||||
K_iff_pz.InputName = {'fm'};
|
||||
K_iff_pz.OutputName = {'f'};
|
||||
|
||||
% RDC
|
||||
K_rdc_vc = -1e3*s;
|
||||
K_rdc_vc.InputName = {'dL'};
|
||||
K_rdc_vc.OutputName = {'f'};
|
||||
K_rdc_md = -1e4*s;
|
||||
K_rdc_md.InputName = {'dL'};
|
||||
K_rdc_md.OutputName = {'f'};
|
||||
K_rdc_pz = -1e5*s;
|
||||
K_rdc_pz.InputName = {'dL'};
|
||||
K_rdc_pz.OutputName = {'f'};
|
||||
|
||||
% DVF
|
||||
K_dvf_vc = -tf(1e3);
|
||||
K_dvf_vc.InputName = {'vn'};
|
||||
K_dvf_vc.OutputName = {'f'};
|
||||
K_dvf_md = -tf(8e3);
|
||||
K_dvf_md.InputName = {'vn'};
|
||||
K_dvf_md.OutputName = {'f'};
|
||||
K_dvf_pz = -tf(2e5);
|
||||
K_dvf_pz.InputName = {'vn'};
|
||||
K_dvf_pz.OutputName = {'f'};
|
||||
|
||||
%% Save Active Damping Controller
|
||||
save('./mat/uniaxial_active_damping_controllers.mat', 'K_iff_vc', 'K_iff_md', 'K_iff_pz', ...
|
||||
'K_rdc_vc', 'K_rdc_md', 'K_rdc_pz', ...
|
||||
'K_dvf_vc', 'K_dvf_md', 'K_dvf_pz');
|
||||
|
||||
%% Compute Damped Plants
|
||||
% IFF
|
||||
G_iff_vc_light = feedback(G_vc_light, K_iff_vc, 'name', +1);
|
||||
G_iff_vc_mid = feedback(G_vc_mid , K_iff_vc, 'name', +1);
|
||||
G_iff_vc_heavy = feedback(G_vc_heavy, K_iff_vc, 'name', +1);
|
||||
G_iff_md_light = feedback(G_md_light, K_iff_md, 'name', +1);
|
||||
G_iff_md_mid = feedback(G_md_mid , K_iff_md, 'name', +1);
|
||||
G_iff_md_heavy = feedback(G_md_heavy, K_iff_md, 'name', +1);
|
||||
G_iff_pz_light = feedback(G_pz_light, K_iff_pz, 'name', +1);
|
||||
G_iff_pz_mid = feedback(G_pz_mid , K_iff_pz, 'name', +1);
|
||||
G_iff_pz_heavy = feedback(G_pz_heavy, K_iff_pz, 'name', +1);
|
||||
|
||||
% RDC
|
||||
G_rdc_vc_light = feedback(G_vc_light, K_rdc_vc, 'name', +1);
|
||||
G_rdc_vc_mid = feedback(G_vc_mid , K_rdc_vc, 'name', +1);
|
||||
G_rdc_vc_heavy = feedback(G_vc_heavy, K_rdc_vc, 'name', +1);
|
||||
G_rdc_md_light = feedback(G_md_light, K_rdc_md, 'name', +1);
|
||||
G_rdc_md_mid = feedback(G_md_mid , K_rdc_md, 'name', +1);
|
||||
G_rdc_md_heavy = feedback(G_md_heavy, K_rdc_md, 'name', +1);
|
||||
G_rdc_pz_light = feedback(G_pz_light, K_rdc_pz, 'name', +1);
|
||||
G_rdc_pz_mid = feedback(G_pz_mid , K_rdc_pz, 'name', +1);
|
||||
G_rdc_pz_heavy = feedback(G_pz_heavy, K_rdc_pz, 'name', +1);
|
||||
|
||||
% DVF
|
||||
G_dvf_vc_light = feedback(G_vc_light, K_dvf_vc, 'name', +1);
|
||||
G_dvf_vc_mid = feedback(G_vc_mid , K_dvf_vc, 'name', +1);
|
||||
G_dvf_vc_heavy = feedback(G_vc_heavy, K_dvf_vc, 'name', +1);
|
||||
G_dvf_md_light = feedback(G_md_light, K_dvf_md, 'name', +1);
|
||||
G_dvf_md_mid = feedback(G_md_mid , K_dvf_md, 'name', +1);
|
||||
G_dvf_md_heavy = feedback(G_md_heavy, K_dvf_md, 'name', +1);
|
||||
G_dvf_pz_light = feedback(G_pz_light, K_dvf_pz, 'name', +1);
|
||||
G_dvf_pz_mid = feedback(G_pz_mid , K_dvf_pz, 'name', +1);
|
||||
G_dvf_pz_heavy = feedback(G_pz_heavy, K_dvf_pz, 'name', +1);
|
||||
|
||||
%% Verify Stability
|
||||
% IFF
|
||||
isstable(G_iff_vc_light) && isstable(G_iff_vc_mid) && isstable(G_iff_vc_heavy) && ...
|
||||
isstable(G_iff_md_light) && isstable(G_iff_md_mid) && isstable(G_iff_md_heavy) && ...
|
||||
isstable(G_iff_pz_light) && isstable(G_iff_pz_mid) && isstable(G_iff_pz_heavy)
|
||||
|
||||
% RDC
|
||||
isstable(G_rdc_vc_light) && isstable(G_rdc_vc_mid) && isstable(G_rdc_vc_heavy) && ...
|
||||
isstable(G_rdc_md_light) && isstable(G_rdc_md_mid) && isstable(G_rdc_md_heavy) && ...
|
||||
isstable(G_rdc_pz_light) && isstable(G_rdc_pz_mid) && isstable(G_rdc_pz_heavy)
|
||||
|
||||
% DVF
|
||||
isstable(G_dvf_vc_light) && isstable(G_dvf_vc_mid) && isstable(G_dvf_vc_heavy) && ...
|
||||
isstable(G_dvf_md_light) && isstable(G_dvf_md_mid) && isstable(G_dvf_md_heavy) && ...
|
||||
isstable(G_dvf_pz_light) && isstable(G_dvf_pz_mid) && isstable(G_dvf_pz_heavy)
|
||||
|
||||
%% Save Damped Plants
|
||||
save('./mat/uniaxial_damped_plants.mat', 'G_iff_vc_light', 'G_iff_md_light', 'G_iff_pz_light', ...
|
||||
'G_rdc_vc_light', 'G_rdc_md_light', 'G_rdc_pz_light', ...
|
||||
'G_dvf_vc_light', 'G_dvf_md_light', 'G_dvf_pz_light', ...
|
||||
'G_iff_vc_mid', 'G_iff_md_mid', 'G_iff_pz_mid', ...
|
||||
'G_rdc_vc_mid', 'G_rdc_md_mid', 'G_rdc_pz_mid', ...
|
||||
'G_dvf_vc_mid', 'G_dvf_md_mid', 'G_dvf_pz_mid', ...
|
||||
'G_iff_vc_heavy', 'G_iff_md_heavy', 'G_iff_pz_heavy', ...
|
||||
'G_rdc_vc_heavy', 'G_rdc_md_heavy', 'G_rdc_pz_heavy', ...
|
||||
'G_dvf_vc_heavy', 'G_dvf_md_heavy', 'G_dvf_pz_heavy');
|
||||
|
||||
%% Active Damping Robustness to change of sample's mass - Root Locus for all three damping techniques with 3 different sample's masses
|
||||
% Soft Nano-Hexapod
|
||||
figure;
|
||||
hold on;
|
||||
% IFF
|
||||
plot(real(pole(G_vc_light('fm', 'f'))), imag(pole(G_vc_light('fm', 'f'))), 'x', 'color', colors(1,:), ...
|
||||
'HandleVisibility', 'off');
|
||||
plot(real(zero(G_vc_light('fm', 'f'))), imag(zero(G_vc_light('fm', 'f'))), 'o', 'color', colors(1,:), ...
|
||||
'DisplayName', 'IFF');
|
||||
for g = logspace(0,2,400)
|
||||
clpoles = pole(feedback(G_vc_light('fm', 'f'), g/s, +1));
|
||||
plot(real(clpoles), imag(clpoles), '.', 'color', colors(1,:), ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
|
||||
% RDC
|
||||
plot(real(pole(G_vc_light('dL', 'f'))), imag(pole(G_vc_light('dL', 'f'))), 'x', 'color', colors(2,:), ...
|
||||
'HandleVisibility', 'off');
|
||||
plot(real(zero(G_vc_light('dL', 'f'))), imag(zero(G_vc_light('dL', 'f'))), 'o', 'color', colors(2,:), ...
|
||||
'DisplayName', 'RDC');
|
||||
for g = logspace(1,3,400)
|
||||
clpoles = pole(feedback(G_vc_light('dL', 'f'), -g*s, +1));
|
||||
plot(real(clpoles), imag(clpoles), '.', 'color', colors(2,:), ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
|
||||
% DVF
|
||||
plot(real(pole(G_vc_light('vn', 'f'))), imag(pole(G_vc_light('vn', 'f'))), 'x', 'color', colors(3,:), ...
|
||||
'HandleVisibility', 'off');
|
||||
plot(real(zero(G_vc_light('vn', 'f'))), imag(zero(G_vc_light('vn', 'f'))), 'o', 'color', colors(3,:), ...
|
||||
'DisplayName', 'DVF');
|
||||
for g = logspace(1,3,400)
|
||||
clpoles = pole(feedback(G_vc_light('vn', 'f'), -g, +1));
|
||||
plot(real(clpoles), imag(clpoles), '.', 'color', colors(3,:), ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
hold off;
|
||||
axis square;
|
||||
xlabel('Real Part'); ylabel('Imaginary Part');
|
||||
ldg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
||||
ldg.ItemTokenSize = [10, 1];
|
||||
xlim([-30, 0]); ylim([0, 30]);
|
||||
ytickangle(90)
|
||||
|
||||
% Medium-Stiff Nano-Hexapod
|
||||
figure;
|
||||
hold on;
|
||||
% IFF
|
||||
plot(real(pole(G_md_light('fm', 'f'))), imag(pole(G_md_light('fm', 'f'))), 'x', 'color', colors(1,:), ...
|
||||
'HandleVisibility', 'off');
|
||||
plot(real(zero(G_md_light('fm', 'f'))), imag(zero(G_md_light('fm', 'f'))), 'o', 'color', colors(1,:), ...
|
||||
'HandleVisibility', 'off');
|
||||
for g = logspace(0,3,400)
|
||||
clpoles = pole(feedback(G_md_light('fm', 'f'), g/s, +1));
|
||||
plot(real(clpoles), imag(clpoles), '.', 'color', colors(1,:), ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
|
||||
% RDC
|
||||
plot(real(pole(G_md_light('dL', 'f'))), imag(pole(G_md_light('dL', 'f'))), 'x', 'color', colors(2,:), ...
|
||||
'HandleVisibility', 'off');
|
||||
plot(real(zero(G_md_light('dL', 'f'))), imag(zero(G_md_light('dL', 'f'))), 'o', 'color', colors(2,:), ...
|
||||
'HandleVisibility', 'off');
|
||||
for g = logspace(2,4,400)
|
||||
clpoles = pole(feedback(G_md_light('dL', 'f'), -g*s, +1));
|
||||
plot(real(clpoles), imag(clpoles), '.', 'color', colors(2,:), ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
|
||||
% DVF
|
||||
plot(real(pole(G_md_light('vn', 'f'))), imag(pole(G_md_light('vn', 'f'))), 'x', 'color', colors(3,:), ...
|
||||
'HandleVisibility', 'off');
|
||||
plot(real(zero(G_md_light('vn', 'f'))), imag(zero(G_md_light('vn', 'f'))), 'o', 'color', colors(3,:), ...
|
||||
'HandleVisibility', 'off');
|
||||
for g = logspace(2,4,400)
|
||||
clpoles = pole(feedback(G_md_light('vn', 'f'), -g, +1));
|
||||
plot(real(clpoles), imag(clpoles), '.', 'color', colors(3,:), ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
hold off;
|
||||
axis square;
|
||||
xlabel('Real Part'); ylabel('Imaginary Part');
|
||||
xlim([-300, 0]); ylim([0, 300]);
|
||||
ytickangle(90)
|
||||
|
||||
% Stiff Nano Hexapod
|
||||
figure;
|
||||
hold on;
|
||||
% IFF
|
||||
plot(real(pole(G_pz_light('fm', 'f'))), imag(pole(G_pz_light('fm', 'f'))), 'x', 'color', colors(1,:), ...
|
||||
'HandleVisibility', 'off');
|
||||
plot(real(zero(G_pz_light('fm', 'f'))), imag(zero(G_pz_light('fm', 'f'))), 'o', 'color', colors(1,:), ...
|
||||
'HandleVisibility', 'off');
|
||||
for g = logspace(2,5,400)
|
||||
clpoles = pole(feedback(G_pz_light('fm', 'f'), g/s, +1));
|
||||
plot(real(clpoles), imag(clpoles), '.', 'color', colors(1,:), ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
|
||||
% RDC
|
||||
plot(real(pole(G_pz_light('dL', 'f'))), imag(pole(G_pz_light('dL', 'f'))), 'x', 'color', colors(2,:), ...
|
||||
'HandleVisibility', 'off');
|
||||
plot(real(zero(G_pz_light('dL', 'f'))), imag(zero(G_pz_light('dL', 'f'))), 'o', 'color', colors(2,:), ...
|
||||
'HandleVisibility', 'off');
|
||||
for g = logspace(3,6,400)
|
||||
clpoles = pole(feedback(G_pz_light('dL', 'f'), -g*s, +1));
|
||||
plot(real(clpoles), imag(clpoles), '.', 'color', colors(2,:), ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
|
||||
% DVF
|
||||
plot(real(pole(G_pz_light('vn', 'f'))), imag(pole(G_pz_light('vn', 'f'))), 'x', 'color', colors(3,:), ...
|
||||
'HandleVisibility', 'off');
|
||||
plot(real(zero(G_pz_light('vn', 'f'))), imag(zero(G_pz_light('vn', 'f'))), 'o', 'color', colors(3,:), ...
|
||||
'HandleVisibility', 'off');
|
||||
for g = logspace(3,6,400)
|
||||
clpoles = pole(feedback(G_pz_light('vn', 'f'), -g, +1));
|
||||
plot(real(clpoles), imag(clpoles), '.', 'color', colors(3,:), ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
hold off;
|
||||
axis square;
|
||||
xlabel('Real Part'); ylabel('Imaginary Part');
|
||||
xlim([-4000, 0]); ylim([0, 4000]);
|
||||
ytickangle(90)
|
||||
|
||||
%% Root Locus for the three damping techniques
|
||||
|
||||
figure;
|
||||
|
||||
hold on;
|
||||
% IFF
|
||||
plot(real(pole(G_md_mid('fm', 'f'))), imag(pole(G_md_mid('fm', 'f'))), 'x', 'color', colors(1,:), ...
|
||||
'DisplayName', 'IFF');
|
||||
plot(real(zero(G_md_mid('fm', 'f'))), imag(zero(G_md_mid('fm', 'f'))), 'o', 'color', colors(1,:), ...
|
||||
'HandleVisibility', 'off');
|
||||
for g = logspace(1,4,500)
|
||||
clpoles = pole(feedback(G_md_mid('fm', 'f'), g/s, +1));
|
||||
plot(real(clpoles), imag(clpoles), '.', 'color', colors(1,:), ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
|
||||
% RDC
|
||||
plot(real(pole(G_md_mid('dL', 'f'))), imag(pole(G_md_mid('dL', 'f'))), 'x', 'color', colors(2,:), ...
|
||||
'DisplayName', 'RDC');
|
||||
plot(real(zero(G_md_mid('dL', 'f'))), imag(zero(G_md_mid('dL', 'f'))), 'o', 'color', colors(2,:), ...
|
||||
'HandleVisibility', 'off');
|
||||
% Estimate the maximum damping added by RDC
|
||||
gs = logspace(2,5,500);
|
||||
phis = zeros(size(gs));
|
||||
for i = 1:length(gs)
|
||||
g = gs(i);
|
||||
clpoles = pole(feedback(G_md_mid('dL', 'f'), -g*s, +1));
|
||||
plot(real(clpoles), imag(clpoles), '.', 'color', colors(2,:), ...
|
||||
'HandleVisibility', 'off');
|
||||
% Estimate damping of u-station mode
|
||||
ustation_pole = clpoles(imag(clpoles)>1000);
|
||||
phis(i) = atan2(abs(real(ustation_pole)), abs(imag(ustation_pole)));
|
||||
end
|
||||
[~, i_max] = max(phis);
|
||||
plot([0, -5e3*sin(phis(i_max))], [0, 5e3*cos(phis(i_max))], 'k--', 'HandleVisibility', 'off');
|
||||
clpoles_max = pole(feedback(G_md_mid('dL', 'f'), -gs(i_max)*s, +1));
|
||||
ustation_pole = clpoles_max(imag(clpoles_max)>1000);
|
||||
plot(real(ustation_pole), imag(ustation_pole), 'kx', ...
|
||||
'HandleVisibility', 'off');
|
||||
% Plot angle
|
||||
plot(-8e2*sin(0:0.01:max(phis)), 8e2*cos(sin(0:0.01:max(phis))), 'k-', 'HandleVisibility', 'off')
|
||||
text(-200, 850, '$\phi$', 'horizontalalignment', 'center');
|
||||
text(real(ustation_pole)-100, imag(ustation_pole), '$\xi = \sin(\phi)$', 'horizontalalignment', 'right');
|
||||
|
||||
% DVF
|
||||
plot(real(pole(G_md_mid('vn', 'f'))), imag(pole(G_md_mid('vn', 'f'))), 'x', 'color', colors(3,:), ...
|
||||
'DisplayName', 'DVF');
|
||||
plot(real(zero(G_md_mid('vn', 'f'))), imag(zero(G_md_mid('vn', 'f'))), 'o', 'color', colors(3,:), ...
|
||||
'HandleVisibility', 'off');
|
||||
for g = logspace(2,5,500)
|
||||
clpoles = pole(feedback(G_md_mid('vn', 'f'), -tf(g), +1));
|
||||
plot(real(clpoles), imag(clpoles), '.', 'color', colors(3,:), ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
hold off;
|
||||
xlim([-2100, 0]); ylim([0, 2100]);
|
||||
axis square;
|
||||
xlabel('Real Part'); ylabel('Imaginary Part');
|
||||
ldg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
||||
ldg.ItemTokenSize = [10, 1];
|
||||
|
||||
%% Obtained damped transfer function from f to d for the three damping techniques
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_mid('d', 'f'), freqs, 'Hz'))), 'k-', 'DisplayName', 'OL');
|
||||
plot(freqs, abs(squeeze(freqresp(G_iff_vc_mid('d', 'f'), freqs, 'Hz'))), 'color', colors(1,:), 'DisplayName', 'IFF');
|
||||
plot(freqs, abs(squeeze(freqresp(G_rdc_vc_mid('d', 'f'), freqs, 'Hz'))), 'color', colors(2,:), 'DisplayName', 'RDC');
|
||||
plot(freqs, abs(squeeze(freqresp(G_dvf_vc_mid('d', 'f'), freqs, 'Hz'))), 'color', colors(3,:), 'DisplayName', 'DVF');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude $d/f$ [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
|
||||
ax2 = nexttile();
|
||||
hold on;
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_mid('d', 'f'), freqs, 'Hz')))), 'k-');
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_vc_mid('d', 'f'), freqs, 'Hz')))), 'color', colors(1,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_rdc_vc_mid('d', 'f'), freqs, 'Hz')))), 'color', colors(2,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_dvf_vc_mid('d', 'f'), freqs, 'Hz')))), 'color', colors(3,:));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
yticks(-360:90:360);
|
||||
ylim([-270, 90]);
|
||||
xticks([1e0, 1e1, 1e2]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([1, 500]);
|
||||
|
||||
%% Obtained damped transfer function from f to d for the three damping techniques
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_mid('d', 'f'), freqs, 'Hz'))), 'k-', 'DisplayName', 'OL');
|
||||
plot(freqs, abs(squeeze(freqresp(G_iff_md_mid('d', 'f'), freqs, 'Hz'))), 'color', colors(1,:), 'DisplayName', 'IFF');
|
||||
plot(freqs, abs(squeeze(freqresp(G_rdc_md_mid('d', 'f'), freqs, 'Hz'))), 'color', colors(2,:), 'DisplayName', 'RDC');
|
||||
plot(freqs, abs(squeeze(freqresp(G_dvf_md_mid('d', 'f'), freqs, 'Hz'))), 'color', colors(3,:), 'DisplayName', 'DVF');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude $d/f$ [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
|
||||
ax2 = nexttile();
|
||||
hold on;
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_md_mid('d', 'f'), freqs, 'Hz')))), 'k-');
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_md_mid('d', 'f'), freqs, 'Hz')))), 'color', colors(1,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_rdc_md_mid('d', 'f'), freqs, 'Hz')))), 'color', colors(2,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_dvf_md_mid('d', 'f'), freqs, 'Hz')))), 'color', colors(3,:));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
yticks(-360:90:360);
|
||||
ylim([-270, 90]);
|
||||
xticks([1e0, 1e1, 1e2]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([1, 500]);
|
||||
|
||||
%% Obtained damped transfer function from f to d for the three damping techniques
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_mid('d', 'f'), freqs, 'Hz'))), 'k-', 'DisplayName', 'OL');
|
||||
plot(freqs, abs(squeeze(freqresp(G_iff_pz_mid('d', 'f'), freqs, 'Hz'))), 'color', colors(1,:), 'DisplayName', 'IFF');
|
||||
plot(freqs, abs(squeeze(freqresp(G_rdc_pz_mid('d', 'f'), freqs, 'Hz'))), 'color', colors(2,:), 'DisplayName', 'RDC');
|
||||
plot(freqs, abs(squeeze(freqresp(G_dvf_pz_mid('d', 'f'), freqs, 'Hz'))), 'color', colors(3,:), 'DisplayName', 'DVF');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude $d/f$ [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
||||
leg.ItemTokenSize(1) = 15;
|
||||
|
||||
ax2 = nexttile();
|
||||
hold on;
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_mid('d', 'f'), freqs, 'Hz')))), 'k-');
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_pz_mid('d', 'f'), freqs, 'Hz')))), 'color', colors(1,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_rdc_pz_mid('d', 'f'), freqs, 'Hz')))), 'color', colors(2,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_dvf_pz_mid('d', 'f'), freqs, 'Hz')))), 'color', colors(3,:));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
yticks(-360:90:360);
|
||||
ylim([-270, 90]);
|
||||
xticks([1e0, 1e1, 1e2]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([1, 500]);
|
||||
|
||||
%% Change of sensitivity to disturbance with all three active damping strategies
|
||||
% FS
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_mid('d', 'fs'), freqs, 'Hz'))), 'k-');
|
||||
plot(freqs, abs(squeeze(freqresp(G_iff_md_mid('d', 'fs'), freqs, 'Hz'))), 'color', colors(1,:));
|
||||
plot(freqs, abs(squeeze(freqresp(G_rdc_md_mid('d', 'fs'), freqs, 'Hz'))), 'color', colors(2,:));
|
||||
plot(freqs, abs(squeeze(freqresp(G_dvf_md_mid('d', 'fs'), freqs, 'Hz'))), 'color', colors(3,:));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude $d/f_{s}$ [m/N]'); xlabel('Frequency [Hz]');
|
||||
xticks([1e0, 1e1, 1e2]);
|
||||
xlim([1, 500]);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_mid('d', 'ft'), freqs, 'Hz'))), 'k-');
|
||||
plot(freqs, abs(squeeze(freqresp(G_iff_md_mid('d', 'ft'), freqs, 'Hz'))), 'color', colors(1,:));
|
||||
plot(freqs, abs(squeeze(freqresp(G_rdc_md_mid('d', 'ft'), freqs, 'Hz'))), 'color', colors(2,:));
|
||||
plot(freqs, abs(squeeze(freqresp(G_dvf_md_mid('d', 'ft'), freqs, 'Hz'))), 'color', colors(3,:));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude $d/f_{t}$ [m/N]'); xlabel('Frequency [Hz]');
|
||||
xticks([1e0, 1e1, 1e2]);
|
||||
xlim([1, 500]);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_mid('d', 'xf'), freqs, 'Hz'))), 'k-', 'DisplayName', 'OL');
|
||||
plot(freqs, abs(squeeze(freqresp(G_iff_md_mid('d', 'xf'), freqs, 'Hz'))), 'color', colors(1,:), 'DisplayName', 'IFF');
|
||||
plot(freqs, abs(squeeze(freqresp(G_rdc_md_mid('d', 'xf'), freqs, 'Hz'))), 'color', colors(2,:), 'DisplayName', 'RDC');
|
||||
plot(freqs, abs(squeeze(freqresp(G_dvf_md_mid('d', 'xf'), freqs, 'Hz'))), 'color', colors(3,:), 'DisplayName', 'DVF');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude $d/x_{f}$ [m/m]'); xlabel('Frequency [Hz]');
|
||||
xticks([1e0, 1e1, 1e2]);
|
||||
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
||||
xlim([1, 500]);
|
||||
|
||||
%% Cumulative Amplitude Spectrum of the distance d with all three active damping techniques
|
||||
figure;
|
||||
hold on;
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_vc_mid('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_vc_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'color', 'black', 'DisplayName', 'OL');
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_iff_vc_mid('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_iff_vc_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'color', colors(1,:), 'DisplayName', 'IFF');
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_rdc_vc_mid('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_rdc_vc_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'color', colors(2,:), 'DisplayName', 'RDC');
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_dvf_vc_mid('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_dvf_vc_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'color', colors(3,:), 'DisplayName', 'DVF');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('CAS of $d$ [m]'); xlabel('Frequency [Hz]');
|
||||
xticks([1e0, 1e1, 1e2]);
|
||||
xlim([1, 500]);
|
||||
ylim([2e-10, 3e-6])
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_md_mid('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_md_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'color', 'black', 'DisplayName', 'OL');
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_iff_md_mid('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_iff_md_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'color', colors(1,:), 'DisplayName', 'IFF');
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_rdc_md_mid('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_rdc_md_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'color', colors(2,:), 'DisplayName', 'RDC');
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_dvf_md_mid('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_dvf_md_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'color', colors(3,:), 'DisplayName', 'DVF');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
||||
xticks([1e0, 1e1, 1e2]);
|
||||
xlim([1, 500]);
|
||||
ylim([2e-10, 3e-6])
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_pz_mid('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_pz_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'color', 'black', 'DisplayName', 'OL');
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_iff_pz_mid('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_iff_pz_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'color', colors(1,:), 'DisplayName', 'IFF');
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_rdc_pz_mid('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_rdc_pz_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'color', colors(2,:), 'DisplayName', 'RDC');
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_dvf_pz_mid('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_dvf_pz_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'color', colors(3,:), 'DisplayName', 'DVF');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
||||
xticks([1e0, 1e1, 1e2]);
|
||||
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
||||
xlim([1, 500]);
|
||||
ylim([2e-10, 3e-6])
|
||||
565
A1-nass-uniaxial-model/uniaxial_6_hac_lac.m
Normal file
565
A1-nass-uniaxial-model/uniaxial_6_hac_lac.m
Normal file
@ -0,0 +1,565 @@
|
||||
%% Clear Workspace and Close figures
|
||||
clear; close all; clc;
|
||||
|
||||
%% Intialize Laplace variable
|
||||
s = zpk('s');
|
||||
|
||||
%% Path for functions, data and scripts
|
||||
addpath('./mat/'); % Path for data
|
||||
|
||||
%% Colors for the figures
|
||||
colors = colororder;
|
||||
|
||||
%% Frequency Vector [Hz]
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
%% Load the PSD of disturbances
|
||||
load('uniaxial_disturbance_psd.mat', 'f', 'psd_ft', 'psd_xf');
|
||||
|
||||
%% Load Plants Dynamics
|
||||
load('uniaxial_plants.mat', 'G_vc_light', 'G_md_light', 'G_pz_light', ...
|
||||
'G_vc_mid', 'G_md_mid', 'G_pz_mid', ...
|
||||
'G_vc_heavy', 'G_md_heavy', 'G_pz_heavy');
|
||||
|
||||
%% Load Damped Plants
|
||||
load('uniaxial_damped_plants.mat', 'G_iff_vc_light', 'G_iff_md_light', 'G_iff_pz_light', ...
|
||||
'G_rdc_vc_light', 'G_rdc_md_light', 'G_rdc_pz_light', ...
|
||||
'G_dvf_vc_light', 'G_dvf_md_light', 'G_dvf_pz_light', ...
|
||||
'G_iff_vc_mid', 'G_iff_md_mid', 'G_iff_pz_mid', ...
|
||||
'G_rdc_vc_mid', 'G_rdc_md_mid', 'G_rdc_pz_mid', ...
|
||||
'G_dvf_vc_mid', 'G_dvf_md_mid', 'G_dvf_pz_mid', ...
|
||||
'G_iff_vc_heavy', 'G_iff_md_heavy', 'G_iff_pz_heavy', ...
|
||||
'G_rdc_vc_heavy', 'G_rdc_md_heavy', 'G_rdc_pz_heavy', ...
|
||||
'G_dvf_vc_heavy', 'G_dvf_md_heavy', 'G_dvf_pz_heavy');
|
||||
|
||||
%% Damped plant - Robustness to change of sample's mass
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_light('d', 'f'), freqs, 'Hz'))), 'color', [colors(1,:), 0.5]);
|
||||
plot(freqs, abs(squeeze(freqresp(G_iff_vc_light('d', 'f'), freqs, 'Hz'))), 'color', colors(1,:));
|
||||
plot(freqs, abs(squeeze(freqresp(G_iff_vc_mid( 'd', 'f'), freqs, 'Hz'))), 'color', colors(2,:));
|
||||
plot(freqs, abs(squeeze(freqresp(G_iff_vc_heavy('d', 'f'), freqs, 'Hz'))), 'color', colors(3,:));
|
||||
loglog(10.^(0.4*cos([0:0.01:2*pi])+log10(100)), ...
|
||||
10.^(0.8*sin([0:0.01:2*pi]-pi/4)+log10(8e-8)), 'k--');
|
||||
text(20, 4e-8, sprintf('Small\nInteraction'), 'horizontalalignment', 'center');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
ylim([5e-10, 1e-3]);
|
||||
|
||||
ax1b = nexttile();
|
||||
hold on;
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_light('d', 'f'), freqs, 'Hz')))), 'color', [colors(1,:), 0.5]);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_vc_light('d', 'f'), freqs, 'Hz')))), 'color', colors(1,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_vc_mid( 'd', 'f'), freqs, 'Hz')))), 'color', colors(2,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_vc_heavy('d', 'f'), freqs, 'Hz')))), 'color', colors(3,:));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
xticks([1e0, 1e1, 1e2]);
|
||||
yticks(-360:90:360);
|
||||
ylim([-200, 20]);
|
||||
|
||||
linkaxes([ax1,ax1b],'x');
|
||||
xlim([1, 1e3]);
|
||||
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
ax2 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_light('d', 'f'), freqs, 'Hz'))), 'color', [colors(1,:), 0.5]);
|
||||
plot(freqs, abs(squeeze(freqresp(G_iff_md_light('d', 'f'), freqs, 'Hz'))), 'color', colors(1,:));
|
||||
plot(freqs, abs(squeeze(freqresp(G_iff_md_mid( 'd', 'f'), freqs, 'Hz'))), 'color', colors(2,:));
|
||||
plot(freqs, abs(squeeze(freqresp(G_iff_md_heavy('d', 'f'), freqs, 'Hz'))), 'color', colors(3,:));
|
||||
loglog(10.^(0.4*cos([0:0.01:2*pi])+log10(200)), ...
|
||||
10.^(0.8*sin([0:0.01:2*pi]-pi/4)+log10(2e-8)), 'k--');
|
||||
text(40, 1e-8, sprintf('Small\nInteraction'), 'horizontalalignment', 'center');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
||||
ylim([5e-10, 1e-3]);
|
||||
|
||||
ax2b = nexttile();
|
||||
hold on;
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_md_light('d', 'f'), freqs, 'Hz')))), 'color', [colors(1,:), 0.5]);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_md_light('d', 'f'), freqs, 'Hz')))), 'color', colors(1,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_md_mid( 'd', 'f'), freqs, 'Hz')))), 'color', colors(2,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_md_heavy('d', 'f'), freqs, 'Hz')))), 'color', colors(3,:));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
||||
xticks([1e0, 1e1, 1e2]);
|
||||
yticks(-360:90:360);
|
||||
ylim([-200, 20]);
|
||||
|
||||
linkaxes([ax2,ax2b],'x');
|
||||
xlim([1, 1e3]);
|
||||
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
ax3 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_light('d', 'f'), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], 'DisplayName', '$m_s = 1\,kg$, OL');
|
||||
plot(freqs, abs(squeeze(freqresp(G_iff_pz_light('d', 'f'), freqs, 'Hz'))), 'color', colors(1,:), 'DisplayName', '$m_s = 1\,kg$, IFF');
|
||||
plot(freqs, abs(squeeze(freqresp(G_iff_pz_mid( 'd', 'f'), freqs, 'Hz'))), 'color', colors(2,:), 'DisplayName', '$m_s = 25\,kg$, IFF');
|
||||
plot(freqs, abs(squeeze(freqresp(G_iff_pz_heavy('d', 'f'), freqs, 'Hz'))), 'color', colors(3,:), 'DisplayName', '$m_s = 50\,kg$, IFF');
|
||||
loglog(10.^(0.8*cos([0:0.01:2*pi])+log10(350)), ...
|
||||
10.^(1.2*sin([0:0.01:2*pi])+log10(8e-9)), 'k--', 'HandleVisibility', 'off');
|
||||
text(200, 5e-7, sprintf('$\\mu$ Station\nCoupling'), 'horizontalalignment', 'center');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
||||
ylim([5e-10, 1e-3]);
|
||||
ldg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
||||
ldg.ItemTokenSize = [20, 1];
|
||||
|
||||
ax3b = nexttile();
|
||||
hold on;
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_light('d', 'f'), freqs, 'Hz')))), 'color', [colors(1,:), 0.5]);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_pz_light('d', 'f'), freqs, 'Hz')))), 'color', colors(1,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_pz_mid( 'd', 'f'), freqs, 'Hz')))), 'color', colors(2,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_pz_heavy('d', 'f'), freqs, 'Hz')))), 'color', colors(3,:));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
||||
xticks([1e0, 1e1, 1e2]);
|
||||
yticks(-360:90:360);
|
||||
ylim([-200, 20]);
|
||||
|
||||
linkaxes([ax3,ax3b],'x');
|
||||
xlim([1, 1e3]);
|
||||
|
||||
%% High Authority Controller - Soft Nano-Hexapod
|
||||
% Lead to increase phase margin
|
||||
a = 5; % Amount of phase lead / width of the phase lead / high frequency gain
|
||||
wc = 2*pi*20; % Frequency with the maximum phase lead [rad/s]
|
||||
H_lead = (1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
|
||||
|
||||
% Lag at low frequency
|
||||
H_lag = (s + 2*pi*5)/(s + 2*pi*0.01);
|
||||
|
||||
% Low Pass filter to increase robustness
|
||||
H_lpf = 1/(1 + s/2/pi/200);
|
||||
|
||||
% High Authority Controller
|
||||
K_hac_vc = 4e5 * ... % Gain
|
||||
H_lead * ... % Lead
|
||||
H_lag * ... % Lag
|
||||
H_lpf; % LPF
|
||||
K_hac_vc.InputName = {'d'};
|
||||
K_hac_vc.OutputName = {'f'};
|
||||
|
||||
%% High Authority Controller - Mid Stiffness Nano-Hexapod
|
||||
% Lead to increase phase margin
|
||||
a = 4; % Amount of phase lead / width of the phase lead / high frequency gain
|
||||
wc = 2*pi*70; % Frequency with the maximum phase lead [rad/s]
|
||||
|
||||
H_lead = (1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
|
||||
|
||||
% Lag at low frequency
|
||||
H_lag = ((s + 2*pi*15)/(s + 2*pi*0.01))^2;
|
||||
|
||||
% Low Pass filter to increase robustness
|
||||
H_lpf = 1/(1 + s/2/pi/300);
|
||||
|
||||
% High Authority Controller
|
||||
K_hac_md = 3e6 * ... % Gain
|
||||
H_lead * ... % Lead
|
||||
H_lag * ... % Lag
|
||||
H_lpf; % LPF
|
||||
K_hac_md.InputName = {'d'};
|
||||
K_hac_md.OutputName = {'f'};
|
||||
|
||||
%% High Authority Controller - Stiff Nano-Hexapod
|
||||
% Lead to increase phase margin
|
||||
a = 5; % Amount of phase lead / width of the phase lead / high frequency gain
|
||||
wc = 2*pi*100; % Frequency with the maximum phase lead [rad/s]
|
||||
H_lead = ((1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a))))^2;
|
||||
|
||||
% Integrator
|
||||
H_int = 1/(s + 2*pi*0.01)^2;
|
||||
|
||||
% Low Pass filter to increase robustness
|
||||
H_lpf = 1/(1 + s/2/pi/500);
|
||||
|
||||
% High Authority Controller
|
||||
K_hac_pz = 6e12 * ... % Gain
|
||||
H_lead * ... % Lead
|
||||
H_int * ... % Lag
|
||||
H_lpf; % LPF
|
||||
K_hac_pz.InputName = {'d'};
|
||||
K_hac_pz.OutputName = {'f'};
|
||||
|
||||
%% Save High Authority Controllers
|
||||
save('./mat/uniaxial_high_authority_controllers.mat', ...
|
||||
'K_hac_vc', 'K_hac_md', 'K_hac_pz');
|
||||
|
||||
%% Compute Loop gain for Nyquist Plot
|
||||
L_vc_light = squeeze(freqresp(K_hac_vc*G_iff_vc_light('d', 'f'), freqs, 'Hz'));
|
||||
L_vc_mid = squeeze(freqresp(K_hac_vc*G_iff_vc_mid( 'd', 'f'), freqs, 'Hz'));
|
||||
L_vc_heavy = squeeze(freqresp(K_hac_vc*G_iff_vc_heavy('d', 'f'), freqs, 'Hz'));
|
||||
|
||||
L_md_light = squeeze(freqresp(K_hac_md*G_iff_md_light('d', 'f'), freqs, 'Hz'));
|
||||
L_md_mid = squeeze(freqresp(K_hac_md*G_iff_md_mid( 'd', 'f'), freqs, 'Hz'));
|
||||
L_md_heavy = squeeze(freqresp(K_hac_md*G_iff_md_heavy('d', 'f'), freqs, 'Hz'));
|
||||
|
||||
L_pz_light = squeeze(freqresp(K_hac_pz*G_iff_pz_light('d', 'f'), freqs, 'Hz'));
|
||||
L_pz_mid = squeeze(freqresp(K_hac_pz*G_iff_pz_mid( 'd', 'f'), freqs, 'Hz'));
|
||||
L_pz_heavy = squeeze(freqresp(K_hac_pz*G_iff_pz_heavy('d', 'f'), freqs, 'Hz'));
|
||||
|
||||
%% Nyquist Plot - Hight Authority Controller for all three nano-hexapod stiffnesses and all sample masses
|
||||
figure;
|
||||
hold on;
|
||||
plot(real(L_vc_light), +imag(L_vc_light), '-', 'color', [colors(1,:), 0.5], 'DisplayName', '$k_n = 0.01\,N/\mu m$')
|
||||
plot(real(L_vc_light), -imag(L_vc_light), '-', 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off')
|
||||
plot(real(L_vc_mid ), +imag(L_vc_mid ), '-', 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off')
|
||||
plot(real(L_vc_mid ), -imag(L_vc_mid ), '-', 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off')
|
||||
plot(real(L_vc_heavy), +imag(L_vc_heavy), '-', 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off')
|
||||
plot(real(L_vc_heavy), -imag(L_vc_heavy), '-', 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off')
|
||||
|
||||
plot(real(L_md_light), +imag(L_md_light), '-', 'color', [colors(2,:), 0.5], 'DisplayName', '$k_n = 1\,N/\mu m$')
|
||||
plot(real(L_md_light), -imag(L_md_light), '-', 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off')
|
||||
plot(real(L_md_mid ), +imag(L_md_mid ), '-', 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off')
|
||||
plot(real(L_md_mid ), -imag(L_md_mid ), '-', 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off')
|
||||
plot(real(L_md_heavy), +imag(L_md_heavy), '-', 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off')
|
||||
plot(real(L_md_heavy), -imag(L_md_heavy), '-', 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off')
|
||||
|
||||
plot(real(L_pz_light), +imag(L_pz_light), '-', 'color', [colors(3,:), 0.5], 'DisplayName', '$k_n = 100\,N/\mu m$')
|
||||
plot(real(L_pz_light), -imag(L_pz_light), '-', 'color', [colors(3,:), 0.5], 'HandleVisibility', 'off')
|
||||
plot(real(L_pz_mid ), +imag(L_pz_mid ), '-', 'color', [colors(3,:), 0.5], 'HandleVisibility', 'off')
|
||||
plot(real(L_pz_mid ), -imag(L_pz_mid ), '-', 'color', [colors(3,:), 0.5], 'HandleVisibility', 'off')
|
||||
plot(real(L_pz_heavy), +imag(L_pz_heavy), '-', 'color', [colors(3,:), 0.5], 'HandleVisibility', 'off')
|
||||
plot(real(L_pz_heavy), -imag(L_pz_heavy), '-', 'color', [colors(3,:), 0.5], 'HandleVisibility', 'off')
|
||||
plot(-1, 0, 'kx', 'HandleVisibility', 'off');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Real'); ylabel('Imag');
|
||||
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
||||
xlim([-3.8, 0.2]); ylim([-2, 2]);
|
||||
axis square;
|
||||
|
||||
%% Nyquist Plot - Hight Authority Controller - Soft Nano-Hexapod
|
||||
figure;
|
||||
hold on;
|
||||
plot(real(L_vc_light), +imag(L_vc_light), '-', 'color', colors(1,:), 'DisplayName', '$m_s = 1\,$kg')
|
||||
plot(real(L_vc_light), -imag(L_vc_light), '-', 'color', colors(1,:), 'HandleVisibility', 'off')
|
||||
plot(real(L_vc_mid ), +imag(L_vc_mid ), '-', 'color', colors(2,:), 'DisplayName', '$m_s = 25\,$kg')
|
||||
plot(real(L_vc_mid ), -imag(L_vc_mid ), '-', 'color', colors(2,:), 'HandleVisibility', 'off')
|
||||
plot(real(L_vc_heavy), +imag(L_vc_heavy), '-', 'color', colors(3,:), 'DisplayName', '$m_s = 50\,$kg')
|
||||
plot(real(L_vc_heavy), -imag(L_vc_heavy), '-', 'color', colors(3,:), 'HandleVisibility', 'off')
|
||||
% Minimum modul margin
|
||||
vc_mod_margin = min([min(abs(L_vc_light + 1)), min(abs(L_vc_mid + 1)), min(abs(L_vc_heavy + 1))]);
|
||||
plot(-1 + vc_mod_margin*cos(linspace(0,2*pi,100)), vc_mod_margin*sin(linspace(0,2*pi,100)), 'k-', 'DisplayName', sprintf('$r = %.2f$', vc_mod_margin))
|
||||
plot(-1, 0, 'kx', 'HandleVisibility', 'off');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Real'); ylabel('Imag');
|
||||
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
||||
leg.ItemTokenSize(1) = 15
|
||||
xlim([-3.8, 0.2]); ylim([-2, 2]);
|
||||
axis square;
|
||||
|
||||
%% Nyquist Plot - Hight Authority Controller - Soft Nano-Hexapod
|
||||
figure;
|
||||
hold on;
|
||||
plot(real(L_md_light), +imag(L_md_light), '-', 'color', colors(1,:), 'DisplayName', '$m_s = 1\,$kg')
|
||||
plot(real(L_md_light), -imag(L_md_light), '-', 'color', colors(1,:), 'HandleVisibility', 'off')
|
||||
plot(real(L_md_mid ), +imag(L_md_mid ), '-', 'color', colors(2,:), 'DisplayName', '$m_s = 25\,$kg')
|
||||
plot(real(L_md_mid ), -imag(L_md_mid ), '-', 'color', colors(2,:), 'HandleVisibility', 'off')
|
||||
plot(real(L_md_heavy), +imag(L_md_heavy), '-', 'color', colors(3,:), 'DisplayName', '$m_s = 50\,$kg')
|
||||
plot(real(L_md_heavy), -imag(L_md_heavy), '-', 'color', colors(3,:), 'HandleVisibility', 'off')
|
||||
% Minimum modul margin
|
||||
md_mod_margin = min([min(abs(L_md_light + 1)), min(abs(L_md_mid + 1)), min(abs(L_md_heavy + 1))]);
|
||||
plot(-1 + md_mod_margin*cos(linspace(0,2*pi,100)), md_mod_margin*sin(linspace(0,2*pi,100)), 'k-', 'DisplayName', sprintf('$r = %.2f$', md_mod_margin))
|
||||
plot(-1, 0, 'kx', 'HandleVisibility', 'off');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Real'); ylabel('Imag');
|
||||
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
||||
leg.ItemTokenSize(1) = 15
|
||||
xlim([-3.8, 0.2]); ylim([-2, 2]);
|
||||
axis square;
|
||||
|
||||
%% Nyquist Plot - Hight Authority Controller - Soft Nano-Hexapod
|
||||
figure;
|
||||
hold on;
|
||||
plot(real(L_pz_light), +imag(L_pz_light), '-', 'color', colors(1,:), 'DisplayName', '$m_s = 1\,$kg')
|
||||
plot(real(L_pz_light), -imag(L_pz_light), '-', 'color', colors(1,:), 'HandleVisibility', 'off')
|
||||
plot(real(L_pz_mid ), +imag(L_pz_mid ), '-', 'color', colors(2,:), 'DisplayName', '$m_s = 25\,$kg')
|
||||
plot(real(L_pz_mid ), -imag(L_pz_mid ), '-', 'color', colors(2,:), 'HandleVisibility', 'off')
|
||||
plot(real(L_pz_heavy), +imag(L_pz_heavy), '-', 'color', colors(3,:), 'DisplayName', '$m_s = 50\,$kg')
|
||||
plot(real(L_pz_heavy), -imag(L_pz_heavy), '-', 'color', colors(3,:), 'HandleVisibility', 'off')
|
||||
% Minimum modul margin
|
||||
pz_mod_margin = min([min(abs(L_pz_light + 1)), min(abs(L_pz_mid + 1)), min(abs(L_pz_heavy + 1))]);
|
||||
plot(-1 + pz_mod_margin*cos(linspace(0,2*pi,100)), pz_mod_margin*sin(linspace(0,2*pi,100)), 'k-', 'DisplayName', sprintf('$r = %.2f$', pz_mod_margin))
|
||||
plot(-1, 0, 'kx', 'HandleVisibility', 'off');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Real'); ylabel('Imag');
|
||||
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
||||
leg.ItemTokenSize(1) = 15
|
||||
xlim([-3.8, 0.2]); ylim([-2, 2]);
|
||||
axis square;
|
||||
|
||||
%% Loop Gain - High Authority Controller - Relatively soft nano-hexapod
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(freqs, abs(L_vc_light), 'color', colors(1,:), 'DisplayName', '$m_s = 1\,$kg');
|
||||
plot(freqs, abs(L_vc_mid), 'color', colors(2,:), 'DisplayName', '$m_s = 25\,$kg');
|
||||
plot(freqs, abs(L_vc_heavy), 'color', colors(3,:), 'DisplayName', '$m_s = 50\,$kg');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
|
||||
ylim([1e-3, 1e3]);
|
||||
yticks([1e-2, 1, 1e2])
|
||||
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
||||
leg.ItemTokenSize(1) = 15
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
plot(freqs, 180/pi*unwrap(angle(L_vc_light)), 'color', colors(1,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(L_vc_mid )), 'color', colors(2,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(L_vc_heavy)), 'color', colors(3,:));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||||
hold off;
|
||||
yticks(-360:45:360);
|
||||
ylim([-225, -90]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([1, 500]);
|
||||
xticks([1, 10, 100]);
|
||||
|
||||
%% Loop Gain - High Authority Controller - Relatively stiff nano-hexapod
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(freqs, abs(L_md_light), 'color', colors(1,:), 'DisplayName', '$m_s = 1\,$kg');
|
||||
plot(freqs, abs(L_md_mid), 'color', colors(2,:), 'DisplayName', '$m_s = 25\,$kg');
|
||||
plot(freqs, abs(L_md_heavy), 'color', colors(3,:), 'DisplayName', '$m_s = 50\,$kg');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
|
||||
ylim([1e-3, 1e3]);
|
||||
yticks([1e-2, 1, 1e2])
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
plot(freqs, 180/pi*unwrap(angle(L_md_light)), 'color', colors(1,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(L_md_mid )), 'color', colors(2,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(L_md_heavy)), 'color', colors(3,:));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||||
hold off;
|
||||
yticks(-360:45:360);
|
||||
ylim([-225, -90]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([1, 500]);
|
||||
xticks([1, 10, 100]);
|
||||
|
||||
%% Loop Gain - High Authority Controller - Stiff nano-hexapod
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(freqs, abs(L_pz_light), 'color', colors(1,:), 'DisplayName', '$m_s = 1\,$kg');
|
||||
plot(freqs, abs(L_pz_mid), 'color', colors(2,:), 'DisplayName', '$m_s = 25\,$kg');
|
||||
plot(freqs, abs(L_pz_heavy), 'color', colors(3,:), 'DisplayName', '$m_s = 50\,$kg');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
|
||||
ylim([1e-3, 1e3]);
|
||||
yticks([1e-2, 1, 1e2])
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
plot(freqs, 180/pi*unwrap(angle(L_pz_light)), 'color', colors(1,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(L_pz_mid )), 'color', colors(2,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(L_pz_heavy)), 'color', colors(3,:));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||||
hold off;
|
||||
yticks(-360:45:360);
|
||||
ylim([-225, -90]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([1, 500]);
|
||||
xticks([1, 10, 100]);
|
||||
|
||||
%% Compute Closed Loop Systems
|
||||
G_hac_iff_vc_light = feedback(G_iff_vc_light, K_hac_vc, 'name', -1);
|
||||
G_hac_iff_vc_mid = feedback(G_iff_vc_mid , K_hac_vc, 'name', -1);
|
||||
G_hac_iff_vc_heavy = feedback(G_iff_vc_heavy, K_hac_vc, 'name', -1);
|
||||
|
||||
G_hac_iff_md_light = feedback(G_iff_md_light, K_hac_md, 'name', -1);
|
||||
G_hac_iff_md_mid = feedback(G_iff_md_mid , K_hac_md, 'name', -1);
|
||||
G_hac_iff_md_heavy = feedback(G_iff_md_heavy, K_hac_md, 'name', -1);
|
||||
|
||||
G_hac_iff_pz_light = feedback(G_iff_pz_light, K_hac_pz, 'name', -1);
|
||||
G_hac_iff_pz_mid = feedback(G_iff_pz_mid , K_hac_pz, 'name', -1);
|
||||
G_hac_iff_pz_heavy = feedback(G_iff_pz_heavy, K_hac_pz, 'name', -1);
|
||||
|
||||
%% Verify Stability
|
||||
isstable(G_hac_iff_vc_light) && isstable(G_hac_iff_vc_mid) && isstable(G_hac_iff_vc_heavy)
|
||||
|
||||
isstable(G_hac_iff_md_light) && isstable(G_hac_iff_md_mid) && isstable(G_hac_iff_md_heavy)
|
||||
|
||||
isstable(G_hac_iff_pz_light) && isstable(G_hac_iff_pz_mid) && isstable(G_hac_iff_pz_heavy)
|
||||
|
||||
%% Change of sensitivity to disturbances with LAC and with HAC-LAC
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_mid( 'd', 'fs'), freqs, 'Hz'))), 'k-');
|
||||
plot(freqs, abs(squeeze(freqresp(G_iff_md_mid('d', 'fs'), freqs, 'Hz'))), 'color', colors(1,:));
|
||||
plot(freqs, abs(squeeze(freqresp(G_hac_iff_md_mid('d', 'fs'), freqs, 'Hz'))), 'color', colors(2,:));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xticks([1e0, 1e1, 1e2]);
|
||||
ylabel('Amplitude $d/f_{s}$ [m/N]'); xlabel('Frequency [Hz]');
|
||||
xlim([1, 500]);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_mid( 'd', 'ft'), freqs, 'Hz'))), 'k-');
|
||||
plot(freqs, abs(squeeze(freqresp(G_iff_md_mid('d', 'ft'), freqs, 'Hz'))), 'color', colors(1,:));
|
||||
plot(freqs, abs(squeeze(freqresp(G_hac_iff_md_mid('d', 'ft'), freqs, 'Hz'))), 'color', colors(2,:));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xticks([1e0, 1e1, 1e2]);
|
||||
ylabel('Amplitude $d/f_{t}$ [m/N]'); xlabel('Frequency [Hz]');
|
||||
xlim([1, 500]);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_mid( 'd', 'xf'), freqs, 'Hz'))), 'k-', 'DisplayName', 'OL');
|
||||
plot(freqs, abs(squeeze(freqresp(G_iff_md_mid('d', 'xf'), freqs, 'Hz'))), 'color', colors(1,:), 'DisplayName', 'IFF');
|
||||
plot(freqs, abs(squeeze(freqresp(G_hac_iff_md_mid('d', 'xf'), freqs, 'Hz'))), 'color', colors(2,:), 'DisplayName', 'HAC-IFF');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xticks([1e0, 1e1, 1e2]);
|
||||
ylabel('Amplitude $d/x_{f}$ [m/m]'); xlabel('Frequency [Hz]');
|
||||
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
||||
xlim([1, 500]);
|
||||
|
||||
%% Cumulative Amplitude Spectrum for all three nano-hexapod stiffnesses - Comparison of OL, IFF and HAC-LAC cases
|
||||
figure;
|
||||
hold on;
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_vc_mid('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_vc_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'color', [0,0,0,0.5], 'DisplayName', 'OL');
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_vc_light('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_vc_light('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'color', [0,0,0,0.5], 'HandleVisibility', 'off');
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_vc_heavy('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_vc_heavy('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'color', [0,0,0,0.5], 'HandleVisibility', 'off');
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_iff_vc_mid('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_iff_vc_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'color', [colors(1,:), 0.5], 'DisplayName', 'IFF');
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_iff_vc_light('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_iff_vc_light('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'color', [colors(1,:), 0.5], 'HandleVisibility', 'off');
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_iff_vc_heavy('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_iff_vc_heavy('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'color', [colors(1,:), 0.5], 'HandleVisibility', 'off');
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_vc_mid('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_hac_iff_vc_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'color', [colors(2,:), 0.5], 'DisplayName', 'HAC-IFF');
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_vc_light('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_hac_iff_vc_light('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'color', [colors(2,:), 0.5], 'HandleVisibility', 'off');
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_vc_heavy('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_hac_iff_vc_heavy('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'color', [colors(2,:), 0.5], 'HandleVisibility', 'off');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xticks([1e0, 1e1, 1e2]);
|
||||
ylabel('CAS of $d$ [m]'); xlabel('Frequency [Hz]');
|
||||
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
||||
xlim([1, 500]);
|
||||
ylim([2e-10, 3e-6])
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_md_mid('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_md_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'color', [0,0,0,0.5]);
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_md_light('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_md_light('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'color', [0,0,0,0.5]);
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_md_heavy('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_md_heavy('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'color', [0,0,0,0.5]);
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_iff_md_mid('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_iff_md_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'color', [colors(1,:), 0.5]);
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_iff_md_light('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_iff_md_light('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'color', [colors(1,:), 0.5]);
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_iff_md_heavy('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_iff_md_heavy('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'color', [colors(1,:), 0.5]);
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_md_mid('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_hac_iff_md_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'color', [colors(2,:), 0.5]);
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_md_light('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_hac_iff_md_light('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'color', [colors(2,:), 0.5]);
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_md_heavy('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_hac_iff_md_heavy('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'color', [colors(2,:), 0.5]);
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xticks([1e0, 1e1, 1e2]);
|
||||
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
||||
xlim([1, 500]);
|
||||
ylim([2e-10, 3e-6])
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_pz_mid('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_pz_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'color', [0,0,0,0.5]);
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_pz_light('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_pz_light('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'color', [0,0,0,0.5]);
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_pz_heavy('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_pz_heavy('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'color', [0,0,0,0.5]);
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_iff_pz_mid('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_iff_pz_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'color', [colors(1,:), 0.5]);
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_iff_pz_light('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_iff_pz_light('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'color', [colors(1,:), 0.5]);
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_iff_pz_heavy('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_iff_pz_heavy('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'color', [colors(1,:), 0.5]);
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_pz_mid('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_hac_iff_pz_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'color', [colors(2,:), 0.5]);
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_pz_light('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_hac_iff_pz_light('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'color', [colors(2,:), 0.5]);
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_pz_heavy('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_hac_iff_pz_heavy('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'color', [colors(2,:), 0.5]);
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xticks([1e0, 1e1, 1e2]);
|
||||
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
||||
xlim([1, 500]);
|
||||
ylim([2e-10, 3e-6])
|
||||
202
A1-nass-uniaxial-model/uniaxial_7_support_compliance.m
Normal file
202
A1-nass-uniaxial-model/uniaxial_7_support_compliance.m
Normal file
@ -0,0 +1,202 @@
|
||||
%% Clear Workspace and Close figures
|
||||
clear; close all; clc;
|
||||
|
||||
%% Intialize Laplace variable
|
||||
s = zpk('s');
|
||||
|
||||
%% Path for functions, data and scripts
|
||||
addpath('./mat/'); % Path for data
|
||||
|
||||
%% Colors for the figures
|
||||
colors = colororder;
|
||||
|
||||
%% Load the micro-station parameters
|
||||
load('uniaxial_micro_station_parameters.mat')
|
||||
|
||||
%% Frequency Vector [Hz]
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
%% Nano-Hexapod Parameters
|
||||
m = 20; % Mass [kg]
|
||||
|
||||
% "Soft" Nano-Hexapod
|
||||
k_soft = m*(2*pi*10)^2; % Stiffness [N/m]
|
||||
c_soft = 0.1*2*sqrt(m*k_soft); % Damping [N/(m/s)]
|
||||
|
||||
% "Mid" Nano-Hexapod
|
||||
k_mid = m*(2*pi*70)^2; % Stiffness [N/m]
|
||||
c_mid = 0.1*2*sqrt(m*k_mid); % Damping [N/(m/s)]
|
||||
|
||||
% "Stiff" Nano-Hexapod
|
||||
k_stiff = m*(2*pi*350)^2; % Stiffness [N/m]
|
||||
c_stiff = 0.1*2*sqrt(m*k_stiff); % Damping [N/(m/s)]
|
||||
|
||||
%% Compute the transfer functions for considered nano-hexapods - From F to L'
|
||||
% "Soft" Nano-Hexapod
|
||||
G_soft_a = 1/(m*s^2 + c_soft*s + k_soft); % Transfer function from F to L'
|
||||
|
||||
% "Mid" Nano-Hexapod
|
||||
G_mid_a = 1/(m*s^2 + c_mid*s + k_mid); % Transfer function from F to L'
|
||||
|
||||
% "Stiff" Nano-Hexapod
|
||||
G_stiff_a = 1/(m*s^2 + c_stiff*s + k_stiff); % Transfer function from F to L'
|
||||
|
||||
%% Obtained transfer functions from F to L when neglecting support compliance
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_soft_a, freqs, 'Hz'))), '-', 'color', colors(1,:));
|
||||
text(50, 5e-5, '$\omega_n =$ 10Hz', 'horizontalalignment', 'center');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Magnitude [m/N]');
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
xticks([1e0, 1e1, 1e2]);
|
||||
ylim([1e-9, 1e-4]);
|
||||
yticks([1e-9, 1e-7, 1e-5]);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_mid_a, freqs, 'Hz'))), '-', 'color', colors(1,:));
|
||||
text(70, 3e-6, '$\omega_n =$ 70Hz', 'horizontalalignment', 'center');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
xticks([1e0, 1e1, 1e2]);
|
||||
ylim([1e-9, 1e-4]);
|
||||
yticks([1e-9, 1e-7, 1e-5]);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_stiff_a, freqs, 'Hz'))), '-', 'color', colors(1,:), ...
|
||||
'DisplayName', '$L^\prime/F$');
|
||||
text(200, 8e-8, '$\omega_n =$ 400Hz', 'horizontalalignment', 'center');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
||||
legend('location', 'northeast');
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
xticks([1e0, 1e1, 1e2]);
|
||||
ylim([1e-9, 1e-4]);
|
||||
yticks([1e-9, 1e-7, 1e-5]);
|
||||
|
||||
%% Parameters of the support compliance
|
||||
w0h = 2*pi*70; % [rad/s]
|
||||
xih = 0.1; % [-]
|
||||
|
||||
mh = 20; % [kg]
|
||||
kh = mh*w0h^2;
|
||||
ch = xih*2*sqrt(kh*mh);
|
||||
|
||||
%% Compute the transfer functions from F to L and from F to d for considered Nano-Hexapods
|
||||
% "Soft" Nano-Hexapod
|
||||
G_soft = (mh*s^2 + ch*s + kh)/(m*s^2*(c_soft*s + k_soft) + (m*s^2 + c_soft*s + k_soft)*(mh*s^2 + ch*s + kh)); % d/F
|
||||
G_soft_r = (1 - m*s^2*G_soft)/(c_soft*s + k_soft); % L/F
|
||||
|
||||
% "Mid" Nano-Hexapod
|
||||
G_mid = (mh*s^2 + ch*s + kh)/(m*s^2*(c_mid*s + k_mid) + (m*s^2 + c_mid*s + k_mid)*(mh*s^2 + ch*s + kh)); % d/F
|
||||
G_mid_r = (1 - m*s^2*G_mid)/(c_mid*s + k_mid); % L/F
|
||||
|
||||
% "Stiff" Nano-Hexapod
|
||||
G_stiff = (mh*s^2 + ch*s + kh)/(m*s^2*(c_stiff*s + k_stiff) + (m*s^2 + c_stiff*s + k_stiff)*(mh*s^2 + ch*s + kh)); % d/F
|
||||
G_stiff_r = (1 - m*s^2*G_stiff)/(c_stiff*s + k_stiff); % L/F
|
||||
|
||||
%% Effect of the support compliance on the transfer functions from F to L and from F to d
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_soft_a, freqs, 'Hz'))), '-', 'color', colors(1,:));
|
||||
plot(freqs, abs(squeeze(freqresp(G_soft_r, freqs, 'Hz'))), '-', 'color', colors(2,:));
|
||||
loglog(10.^(0.3*cos(0:0.01:2*pi)+log10(60)), ...
|
||||
10.^(0.6*sin(0:0.01:2*pi)+log10(4e-7)), 'k--');
|
||||
text(8, 3e-7, sprintf('Support\nDynamics'), 'horizontalalignment', 'center');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Magnitude [m/N]');
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
xticks([1e0, 1e1, 1e2]);
|
||||
ylim([1e-9, 1e-4]);
|
||||
yticks([1e-9, 1e-7, 1e-5]);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_mid_a, freqs, 'Hz'))), '-', 'color', colors(1,:));
|
||||
plot(freqs, abs(squeeze(freqresp(G_mid_r, freqs, 'Hz'))), '-', 'color', colors(2,:));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]');
|
||||
set(gca, 'YTickLabel',[]);
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
xticks([1e0, 1e1, 1e2]);
|
||||
ylim([1e-9, 1e-4]);
|
||||
yticks([1e-9, 1e-7, 1e-5]);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_stiff_a, freqs, 'Hz'))), '-', 'color', colors(1,:), ...
|
||||
'DisplayName', '$L^\prime/F$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_stiff_r, freqs, 'Hz'))), '-', 'color', colors(2,:), ...
|
||||
'DisplayName', '$L/F$');
|
||||
loglog(10.^(0.3*cos(0:0.01:2*pi)+log10(50)), ...
|
||||
10.^(0.3*sin(0:0.01:2*pi)+log10(8e-9)), 'k--', 'HandleVisibility', 'off');
|
||||
text(50, 3e-8, 'No effect', 'horizontalalignment', 'center');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
||||
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
xticks([1e0, 1e1, 1e2]);
|
||||
ylim([1e-9, 1e-4]);
|
||||
yticks([1e-9, 1e-7, 1e-5]);
|
||||
|
||||
%% Effect of the support compliance on the transfer functions from F to L and from F to d
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_soft_a, freqs, 'Hz'))), '-', 'color', colors(1,:));
|
||||
plot(freqs, abs(squeeze(freqresp(G_soft, freqs, 'Hz'))), '-', 'color', colors(3,:));
|
||||
loglog(10.^(0.3*cos(0:0.01:2*pi)+log10(60)), ...
|
||||
10.^(0.6*sin(0:0.01:2*pi)+log10(4e-7)), 'k--');
|
||||
text(8, 3e-7, 'No effect', 'horizontalalignment', 'center');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Magnitude [m/N]');
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
xticks([1e0, 1e1, 1e2]);
|
||||
ylim([1e-9, 1e-4]);
|
||||
yticks([1e-9, 1e-7, 1e-5]);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_mid_a, freqs, 'Hz'))), '-', 'color', colors(1,:));
|
||||
plot(freqs, abs(squeeze(freqresp(G_mid, freqs, 'Hz'))), '-', 'color', colors(3,:));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]');
|
||||
set(gca, 'YTickLabel',[]);
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
xticks([1e0, 1e1, 1e2]);
|
||||
ylim([1e-9, 1e-4]);
|
||||
yticks([1e-9, 1e-7, 1e-5]);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_stiff_a, freqs, 'Hz'))), '-', 'color', colors(1,:), ...
|
||||
'DisplayName', '$L^\prime/F$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_stiff, freqs, 'Hz'))), '-', 'color', colors(3,:), ...
|
||||
'DisplayName', '$d/F$');
|
||||
loglog(10.^(0.4*cos(0:0.01:2*pi)+log10(50)), ...
|
||||
10.^(0.8*sin(0:0.01:2*pi)+log10(8e-9)), 'k--', 'HandleVisibility', 'off');
|
||||
text(50, 2e-7, sprintf('Support\nDynamics'), 'horizontalalignment', 'center');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
||||
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
xticks([1e0, 1e1, 1e2]);
|
||||
ylim([1e-9, 1e-4]);
|
||||
yticks([1e-9, 1e-7, 1e-5]);
|
||||
397
A1-nass-uniaxial-model/uniaxial_8_payload_dynamics.m
Normal file
397
A1-nass-uniaxial-model/uniaxial_8_payload_dynamics.m
Normal file
@ -0,0 +1,397 @@
|
||||
%% Clear Workspace and Close figures
|
||||
clear; close all; clc;
|
||||
|
||||
%% Intialize Laplace variable
|
||||
s = zpk('s');
|
||||
|
||||
%% Path for functions, data and scripts
|
||||
addpath('./mat/'); % Path for data
|
||||
|
||||
%% Colors for the figures
|
||||
colors = colororder;
|
||||
|
||||
%% Uniaxial Simscape model name
|
||||
mdl = 'nass_uniaxial_model';
|
||||
|
||||
%% Load the micro-station parameters
|
||||
load('uniaxial_micro_station_parameters.mat')
|
||||
|
||||
%% Load the PSD of disturbances
|
||||
load('uniaxial_disturbance_psd.mat', 'f', 'psd_ft', 'psd_xf');
|
||||
|
||||
%% Load Active Damping Controller
|
||||
load('uniaxial_active_damping_controllers.mat', 'K_iff_vc', 'K_iff_md', 'K_iff_pz', ...
|
||||
'K_rdc_vc', 'K_rdc_md', 'K_rdc_pz', ...
|
||||
'K_dvf_vc', 'K_dvf_md', 'K_dvf_pz');
|
||||
|
||||
%% Load High Authority Controllers
|
||||
load('uniaxial_high_authority_controllers.mat', 'K_hac_vc', 'K_hac_md', 'K_hac_pz');
|
||||
|
||||
%% Frequency Vector [Hz]
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
%% Soft Nano-Hexapod
|
||||
% Light payload mass
|
||||
mn = 15; % Nano-Hexapod mass [kg]
|
||||
ms = 1; % Sample Mass [kg]
|
||||
kn = 1e4; % Nano-Hexapod (soft) Stiffness [N/m]
|
||||
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
|
||||
|
||||
% Rigid sample
|
||||
G_vc_rigid_light = 1/((mn + ms)*s^2 + cn*s + kn);
|
||||
|
||||
% Soft Sample
|
||||
ws = 2*pi*20;
|
||||
ks = ms * ws^2;
|
||||
cs = 2*0.01*sqrt(ms*ks);
|
||||
G_vc_soft_light = (ms*s^2 + cs*s + ks)/((mn*s^2 + cn*s + kn)*(ms*s^2 + cs*s + ks) + ms*s^2*(cs*s + ks));
|
||||
|
||||
% Stiff Sample
|
||||
ws = 2*pi*200;
|
||||
ks = ms * ws^2;
|
||||
cs = 2*0.01*sqrt(ms*ks);
|
||||
G_vc_stiff_light = (ms*s^2 + cs*s + ks)/((mn*s^2 + cn*s + kn)*(ms*s^2 + cs*s + ks) + ms*s^2*(cs*s + ks));
|
||||
|
||||
% Heavy payload mass
|
||||
mn = 15; % Nano-Hexapod mass [kg]
|
||||
ms = 50; % Sample Mass [kg]
|
||||
kn = 1e4; % Nano-Hexapod (soft) Stiffness [N/m]
|
||||
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
|
||||
|
||||
% Rigid sample
|
||||
G_vc_rigid_heavy = 1/((mn + ms)*s^2 + cn*s + kn);
|
||||
|
||||
% Soft Sample
|
||||
ws = 2*pi*20;
|
||||
ks = ms * ws^2;
|
||||
cs = 2*0.01*sqrt(ms*ks);
|
||||
G_vc_soft_heavy = (ms*s^2 + cs*s + ks)/((mn*s^2 + cn*s + kn)*(ms*s^2 + cs*s + ks) + ms*s^2*(cs*s + ks));
|
||||
|
||||
% Stiff Sample
|
||||
ws = 2*pi*200;
|
||||
ks = ms * ws^2;
|
||||
cs = 2*0.01*sqrt(ms*ks);
|
||||
G_vc_stiff_heavy = (ms*s^2 + cs*s + ks)/((mn*s^2 + cn*s + kn)*(ms*s^2 + cs*s + ks) + ms*s^2*(cs*s + ks));
|
||||
|
||||
%% Effect of the payload dynamics on the soft Nano-Hexapod. Light sample on the right, and heavy sample on the left
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_rigid_light, freqs, 'Hz'))), '-', 'color', colors(1,:), 'DisplayName', 'Rigid sample');
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_stiff_light, freqs, 'Hz'))), '-', 'color', colors(2,:), 'DisplayName', '$\omega_s = 200\,Hz$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_soft_light, freqs, 'Hz'))), '-', 'color', colors(3,:), 'DisplayName', '$\omega_s = 20\,Hz$');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
ylim([1e-10, 1e-2])
|
||||
|
||||
ax1b = nexttile();
|
||||
hold on;
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_rigid_light, freqs, 'Hz')))), '-', 'color', colors(1,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_stiff_light, freqs, 'Hz')))), '-', 'color', colors(2,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_soft_light, freqs, 'Hz')))), '-', 'color', colors(3,:));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||||
xticks([1e0, 1e1, 1e2]);
|
||||
yticks(-360:90:360);
|
||||
ylim([-200, 20]);
|
||||
|
||||
linkaxes([ax1,ax1b],'x');
|
||||
xlim([1, 1000]);
|
||||
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax2 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_rigid_heavy, freqs, 'Hz'))), '-', 'color', colors(1,:), 'DisplayName', 'Rigid sample');
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_stiff_heavy, freqs, 'Hz'))), '-', 'color', colors(2,:), 'DisplayName', '$\omega_s = 200\,Hz$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_soft_heavy, freqs, 'Hz'))), '-', 'color', colors(3,:), 'DisplayName', '$\omega_s = 20\,Hz$');
|
||||
plot(freqs, abs(squeeze(freqresp(1/(mn*s^2), freqs, 'Hz'))), '-', 'color', [0,0,0,0.5], 'DisplayName', '$\frac{1}{m_n s^2}$');
|
||||
plot(freqs, abs(squeeze(freqresp(1/((mn + ms)*s^2), freqs, 'Hz'))), '--', 'color', [0,0,0,0.5], 'DisplayName', '$\frac{1}{(m_n + m_s) s^2}$');
|
||||
text(2.2, 2e-3, '$\omega_n = \sqrt{\frac{k_n}{m_n + m_s}}$', 'horizontalalignment', 'left');
|
||||
text(20, 1e-8, '$\omega_s = \sqrt{\frac{k_s}{m_s}}$', 'horizontalalignment', 'center');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
ldg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
||||
ldg.ItemTokenSize = [20, 1];
|
||||
ylim([1e-10, 1e-2])
|
||||
|
||||
ax2b = nexttile();
|
||||
hold on;
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_rigid_heavy, freqs, 'Hz')))), '-', 'color', colors(1,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_stiff_heavy, freqs, 'Hz')))), '-', 'color', colors(2,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_soft_heavy, freqs, 'Hz')))), '-', 'color', colors(3,:));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||||
xticks([1e0, 1e1, 1e2]);
|
||||
yticks(-360:90:360);
|
||||
ylim([-200, 20]);
|
||||
|
||||
linkaxes([ax2,ax2b],'x');
|
||||
xlim([1, 1000]);
|
||||
|
||||
%% Stiff Nano-Hexapod
|
||||
% Light payload mass
|
||||
mn = 15; % Nano-Hexapod mass [kg]
|
||||
ms = 1; % Sample Mass [kg]
|
||||
kn = 1e8; % Nano-Hexapod (soft) Stiffness [N/m]
|
||||
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
|
||||
|
||||
% Rigid sample
|
||||
G_pz_rigid_light = 1/((mn + ms)*s^2 + cn*s + kn);
|
||||
|
||||
% Soft Sample
|
||||
ws = 2*pi*20;
|
||||
ks = ms * ws^2;
|
||||
cs = 2*0.01*sqrt(ms*ks);
|
||||
G_pz_soft_light = (ms*s^2 + cs*s + ks)/((mn*s^2 + cn*s + kn)*(ms*s^2 + cs*s + ks) + ms*s^2*(cs*s + ks));
|
||||
|
||||
% Stiff Sample
|
||||
ws = 2*pi*200;
|
||||
ks = ms * ws^2;
|
||||
cs = 2*0.01*sqrt(ms*ks);
|
||||
G_pz_stiff_light = (ms*s^2 + cs*s + ks)/((mn*s^2 + cn*s + kn)*(ms*s^2 + cs*s + ks) + ms*s^2*(cs*s + ks));
|
||||
|
||||
% Heavy payload mass
|
||||
mn = 15; % Nano-Hexapod mass [kg]
|
||||
ms = 50; % Sample Mass [kg]
|
||||
kn = 1e8; % Nano-Hexapod (soft) Stiffness [N/m]
|
||||
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
|
||||
|
||||
% Rigid sample
|
||||
G_pz_rigid_heavy = 1/((mn + ms)*s^2 + cn*s + kn);
|
||||
|
||||
% Soft Sample
|
||||
ws = 2*pi*20;
|
||||
ks = ms * ws^2;
|
||||
cs = 2*0.01*sqrt(ms*ks);
|
||||
G_pz_soft_heavy = (ms*s^2 + cs*s + ks)/((mn*s^2 + cn*s + kn)*(ms*s^2 + cs*s + ks) + ms*s^2*(cs*s + ks));
|
||||
|
||||
% Stiff Sample
|
||||
ws = 2*pi*200;
|
||||
ks = ms * ws^2;
|
||||
cs = 2*0.01*sqrt(ms*ks);
|
||||
G_pz_stiff_heavy = (ms*s^2 + cs*s + ks)/((mn*s^2 + cn*s + kn)*(ms*s^2 + cs*s + ks) + ms*s^2*(cs*s + ks));
|
||||
|
||||
%% Effect of the payload dynamics on the stiff Nano-Hexapod. Light sample on the right, and heavy sample on the left
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_rigid_light, freqs, 'Hz'))), '-', 'color', colors(1,:));
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_stiff_light, freqs, 'Hz'))), '-', 'color', colors(2,:));
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_soft_light, freqs, 'Hz'))), '-', 'color', colors(3,:));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
ylim([1e-10, 1e-6])
|
||||
|
||||
ax1b = nexttile();
|
||||
hold on;
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_rigid_light, freqs, 'Hz')))), '-', 'color', colors(1,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_stiff_light, freqs, 'Hz')))), '-', 'color', colors(2,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_soft_light, freqs, 'Hz')))), '-', 'color', colors(3,:));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||||
xticks([1e0, 1e1, 1e2]);
|
||||
yticks(-360:90:360);
|
||||
ylim([-200, 20]);
|
||||
|
||||
linkaxes([ax1,ax1b],'x');
|
||||
xlim([1, 1000]);
|
||||
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax2 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_rigid_heavy, freqs, 'Hz'))), '-', 'color', colors(1,:), 'DisplayName', 'Rigid sample');
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_stiff_heavy, freqs, 'Hz'))), '-', 'color', colors(2,:), 'DisplayName', 'Stiff sample: $\omega_s = 200\,Hz$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_soft_heavy, freqs, 'Hz'))), '-', 'color', colors(3,:), 'DisplayName', 'Soft sample: $\omega_s = 20\,Hz$');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
ylim([1e-10, 1e-6])
|
||||
ldg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
||||
ldg.ItemTokenSize = [20, 1];
|
||||
|
||||
ax2b = nexttile();
|
||||
hold on;
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_rigid_heavy, freqs, 'Hz')))), '-', 'color', colors(1,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_stiff_heavy, freqs, 'Hz')))), '-', 'color', colors(2,:));
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_soft_heavy, freqs, 'Hz')))), '-', 'color', colors(3,:));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||||
xticks([1e0, 1e1, 1e2]);
|
||||
yticks(-360:90:360);
|
||||
ylim([-200, 20]);
|
||||
|
||||
linkaxes([ax2,ax2b],'x');
|
||||
xlim([1, 1000]);
|
||||
|
||||
%% Nano-Hexpod model
|
||||
model_config = struct();
|
||||
model_config.controller = "open_loop";
|
||||
mn = 15; % Nano-Hexapod mass [kg]
|
||||
ms = 1; % Sample Mass [kg]
|
||||
|
||||
%% Identification
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force
|
||||
io(io_i) = linio([mdl, '/micro_station/xf'], 1, 'openinput'); io_i = io_i + 1; % Floor Motion
|
||||
io(io_i) = linio([mdl, '/micro_station/ft'], 1, 'openinput'); io_i = io_i + 1; % Stage vibrations
|
||||
io(io_i) = linio([mdl, '/fs'], 1, 'openinput'); io_i = io_i + 1; % Direct sample forces
|
||||
io(io_i) = linio([mdl, '/dL'], 1, 'openoutput'); io_i = io_i + 1; % Relative Motion Sensor
|
||||
io(io_i) = linio([mdl, '/fm'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensor
|
||||
io(io_i) = linio([mdl, '/vn'] , 1, 'openoutput'); io_i = io_i + 1; % Geophone
|
||||
io(io_i) = linio([mdl, '/d'] , 1, 'openoutput'); io_i = io_i + 1; % Metrology Output
|
||||
io(io_i) = linio([mdl, '/y'] , 1, 'openoutput'); io_i = io_i + 1; % Sample's position
|
||||
|
||||
%% Soft Nano-Hexapod
|
||||
% Light payload mass
|
||||
kn = 1e4; % Nano-Hexapod (soft) Stiffness [N/m]
|
||||
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
|
||||
|
||||
% Rigid Sample
|
||||
model_config.nhexa = "1dof";
|
||||
G_vc_light_rigid = linearize(mdl, io, 0.0);
|
||||
G_vc_light_rigid.InputName = {'f', 'xf', 'ft', 'fs'};
|
||||
G_vc_light_rigid.OutputName = {'dL', 'fm', 'vn', 'd', 'y'};
|
||||
|
||||
% Soft Sample
|
||||
model_config.nhexa = "2dof";
|
||||
ws = 2*pi*20;
|
||||
ks = ms * ws^2;
|
||||
cs = 2*0.01*sqrt(ms*ks);
|
||||
|
||||
G_vc_light_soft = linearize(mdl, io, 0.0);
|
||||
G_vc_light_soft.InputName = {'f', 'xf', 'ft', 'fs'};
|
||||
G_vc_light_soft.OutputName = {'dL', 'fm', 'vn', 'd', 'y'};
|
||||
|
||||
% Rigid Sample
|
||||
model_config.nhexa = "2dof";
|
||||
ws = 2*pi*200;
|
||||
ks = ms * ws^2;
|
||||
cs = 2*0.01*sqrt(ms*ks);
|
||||
|
||||
G_vc_light_stiff = linearize(mdl, io, 0.0);
|
||||
G_vc_light_stiff.InputName = {'f', 'xf', 'ft', 'fs'};
|
||||
G_vc_light_stiff.OutputName = {'dL', 'fm', 'vn', 'd', 'y'};
|
||||
|
||||
%% Stiff Nano-Hexapod
|
||||
% Light payload mass
|
||||
kn = 1e8; % Nano-Hexapod (soft) Stiffness [N/m]
|
||||
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
|
||||
|
||||
% Rigid Sample
|
||||
model_config.nhexa = "1dof";
|
||||
G_pz_light_rigid = linearize(mdl, io, 0.0);
|
||||
G_pz_light_rigid.InputName = {'f', 'xf', 'ft', 'fs'};
|
||||
G_pz_light_rigid.OutputName = {'dL', 'fm', 'vn', 'd', 'y'};
|
||||
|
||||
% Soft Sample
|
||||
model_config.nhexa = "2dof";
|
||||
ws = 2*pi*20;
|
||||
ks = ms * ws^2;
|
||||
cs = 2*0.01*sqrt(ms*ks);
|
||||
|
||||
G_pz_light_soft = linearize(mdl, io, 0.0);
|
||||
G_pz_light_soft.InputName = {'f', 'xf', 'ft', 'fs'};
|
||||
G_pz_light_soft.OutputName = {'dL', 'fm', 'vn', 'd', 'y'};
|
||||
|
||||
% Rigid Sample
|
||||
model_config.nhexa = "2dof";
|
||||
ws = 2*pi*200;
|
||||
ks = ms * ws^2;
|
||||
cs = 2*0.01*sqrt(ms*ks);
|
||||
|
||||
G_pz_light_stiff = linearize(mdl, io, 0.0);
|
||||
G_pz_light_stiff.InputName = {'f', 'xf', 'ft', 'fs'};
|
||||
G_pz_light_stiff.OutputName = {'dL', 'fm', 'vn', 'd', 'y'};
|
||||
|
||||
%% Apply IFF and verify stability
|
||||
% Soft Nano-Hexapod
|
||||
G_iff_vc_light_rigid = feedback(G_vc_light_rigid, K_iff_vc, 'name', +1);
|
||||
G_iff_vc_light_soft = feedback(G_vc_light_soft , K_iff_vc, 'name', +1);
|
||||
G_iff_vc_light_stiff = feedback(G_vc_light_stiff, K_iff_vc, 'name', +1);
|
||||
|
||||
isstable(G_iff_vc_light_rigid)
|
||||
isstable(G_iff_vc_light_soft)
|
||||
isstable(G_iff_vc_light_stiff)
|
||||
|
||||
% Stiff Nano-Hexapod
|
||||
G_iff_pz_light_rigid = feedback(G_pz_light_rigid, K_iff_pz, 'name', +1);
|
||||
G_iff_pz_light_soft = feedback(G_pz_light_soft , K_iff_pz, 'name', +1);
|
||||
G_iff_pz_light_stiff = feedback(G_pz_light_stiff, K_iff_pz, 'name', +1);
|
||||
|
||||
isstable(G_iff_pz_light_rigid)
|
||||
isstable(G_iff_pz_light_soft)
|
||||
isstable(G_iff_pz_light_stiff)
|
||||
|
||||
%% Compute closed-loop plants and verify stability
|
||||
% Soft Nano-Hexapod
|
||||
G_hac_iff_vc_light_rigid = feedback(G_iff_vc_light_rigid, K_hac_vc, 'name', -1);
|
||||
G_hac_iff_vc_light_soft = feedback(G_iff_vc_light_soft , K_hac_vc, 'name', -1);
|
||||
G_hac_iff_vc_light_stiff = feedback(G_iff_vc_light_stiff, K_hac_vc, 'name', -1);
|
||||
|
||||
isstable(G_hac_iff_vc_light_rigid)
|
||||
isstable(G_hac_iff_vc_light_soft)
|
||||
isstable(G_hac_iff_vc_light_stiff)
|
||||
|
||||
% Stiff Nano-Hexapod
|
||||
G_hac_iff_pz_light_rigid = feedback(G_iff_pz_light_rigid, K_hac_pz, 'name', -1);
|
||||
G_hac_iff_pz_light_soft = feedback(G_iff_pz_light_soft , K_hac_pz, 'name', -1);
|
||||
G_hac_iff_pz_light_stiff = feedback(G_iff_pz_light_stiff, K_hac_pz, 'name', -1);
|
||||
|
||||
isstable(G_hac_iff_pz_light_rigid)
|
||||
isstable(G_hac_iff_pz_light_soft)
|
||||
isstable(G_hac_iff_pz_light_stiff)
|
||||
|
||||
%% Cumulative Amplitude Spectrum of d - Effect of Sample's flexibility
|
||||
figure;
|
||||
hold on;
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_pz_light_rigid('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_hac_iff_pz_light_rigid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'DisplayName', 'Rigid sample');
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_pz_light_stiff('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_hac_iff_pz_light_stiff('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'DisplayName', '$\omega_s = 200\,$Hz');
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_pz_light_soft('d', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_hac_iff_pz_light_soft('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'DisplayName', '$\omega_s = 20\,$Hz');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('CAS of $d$ [m]'); xlabel('Frequency [Hz]');
|
||||
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
||||
xlim([1, 500]);
|
||||
xticks([1e0, 1e1, 1e2]);
|
||||
ylim([2e-10, 2e-7])
|
||||
|
||||
%% Cumulative Amplitude Spectrum - Effect of Sample's flexibility
|
||||
figure;
|
||||
hold on;
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_pz_light_rigid('y', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_hac_iff_pz_light_rigid('y', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'DisplayName', 'Rigid sample');
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_pz_light_stiff('y', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_hac_iff_pz_light_stiff('y', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'DisplayName', '$\omega_s = 200\,$Hz');
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_pz_light_soft('y', 'ft'), f, 'Hz'))).^2 + ...
|
||||
psd_xf.*abs(squeeze(freqresp(G_hac_iff_pz_light_soft('y', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
||||
'DisplayName', '$\omega_s = 20\,$Hz');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('CAS of $y$ [m]'); xlabel('Frequency [Hz]');
|
||||
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
||||
xlim([1, 500]);
|
||||
xticks([1e0, 1e1, 1e2]);
|
||||
ylim([2e-10, 2e-7])
|
||||
BIN
A2-nass-rotating-3dof-model/mat/nass_controllers.mat
(Stored with Git LFS)
Normal file
BIN
A2-nass-rotating-3dof-model/mat/nass_controllers.mat
(Stored with Git LFS)
Normal file
Binary file not shown.
BIN
A2-nass-rotating-3dof-model/mat/rotating_generic_plants.mat
(Stored with Git LFS)
Normal file
BIN
A2-nass-rotating-3dof-model/mat/rotating_generic_plants.mat
(Stored with Git LFS)
Normal file
Binary file not shown.
BIN
A2-nass-rotating-3dof-model/mat/rotating_nass_plants.mat
(Stored with Git LFS)
Normal file
BIN
A2-nass-rotating-3dof-model/mat/rotating_nass_plants.mat
(Stored with Git LFS)
Normal file
Binary file not shown.
BIN
A2-nass-rotating-3dof-model/mat/uniaxial_micro_station_parameters.mat
(Stored with Git LFS)
Normal file
BIN
A2-nass-rotating-3dof-model/mat/uniaxial_micro_station_parameters.mat
(Stored with Git LFS)
Normal file
Binary file not shown.
BIN
A2-nass-rotating-3dof-model/nass_rotating_model.slx
Normal file
BIN
A2-nass-rotating-3dof-model/nass_rotating_model.slx
Normal file
Binary file not shown.
187
A2-nass-rotating-3dof-model/rotating_1_system_description.m
Normal file
187
A2-nass-rotating-3dof-model/rotating_1_system_description.m
Normal file
@ -0,0 +1,187 @@
|
||||
%% Clear Workspace and Close figures
|
||||
clear; close all; clc;
|
||||
|
||||
%% Intialize Laplace variable
|
||||
s = zpk('s');
|
||||
|
||||
%% Path for functions, data and scripts
|
||||
addpath('./mat/'); % Path for data
|
||||
addpath('./src/'); % Path for Functions
|
||||
|
||||
%% Colors for the figures
|
||||
colors = colororder;
|
||||
|
||||
%% Simscape model name
|
||||
mdl = 'rotating_model';
|
||||
|
||||
%% Model parameters for first analysis
|
||||
kn = 1; % Actuator Stiffness [N/m]
|
||||
mn = 1; % Payload Mass [kg]
|
||||
cn = 0.05; % Actuator Damping [N/(m/s)]
|
||||
|
||||
xin = cn/(2*sqrt(kn*mn)); % Modal Damping [-]
|
||||
w0n = sqrt(kn/mn); % Natural Frequency [rad/s]
|
||||
|
||||
%% Computation of the poles as a function of the rotating velocity
|
||||
Wzs = linspace(0, 2, 51); % Vector of rotation speeds [rad/s]
|
||||
|
||||
p_ws = zeros(4, length(Wzs));
|
||||
|
||||
for i = 1:length(Wzs)
|
||||
Wz = Wzs(i);
|
||||
|
||||
pole_G = pole(1/(((s^2)/(w0n^2) + 2*xin*s/w0n + 1 - (Wz^2)/(w0n^2))^2 + (2*Wz*s/(w0n^2))^2));
|
||||
[~, i_sort] = sort(imag(pole_G));
|
||||
p_ws(:, i) = pole_G(i_sort);
|
||||
end
|
||||
|
||||
clear pole_G;
|
||||
|
||||
%% Campbell diagram - Real and Imaginary parts of the poles as a function of the rotating velocity
|
||||
figure;
|
||||
hold on;
|
||||
plot(Wzs, real(p_ws(1, :)), '-', 'color', colors(1,:), 'DisplayName', '$p_{+}$')
|
||||
plot(Wzs, real(p_ws(4, :)), '-', 'color', colors(1,:), 'HandleVisibility', 'off')
|
||||
plot(Wzs, real(p_ws(2, :)), '-', 'color', colors(2,:), 'DisplayName', '$p_{-}$')
|
||||
plot(Wzs, real(p_ws(3, :)), '-', 'color', colors(2,:), 'HandleVisibility', 'off')
|
||||
plot(Wzs, zeros(size(Wzs)), 'k--', 'HandleVisibility', 'off')
|
||||
hold off;
|
||||
xlabel('Rotational Speed $\Omega$'); ylabel('Real Part');
|
||||
leg = legend('location', 'northwest', 'FontSize', 8);
|
||||
leg.ItemTokenSize(1) = 8;
|
||||
xlim([0, 2*w0n]);
|
||||
xticks([0,w0n/2,w0n,3/2*w0n,2*w0n])
|
||||
xticklabels({'$0$', '', '$\omega_0$', '', '$2 \omega_0$'})
|
||||
ylim([-3*xin, 3*xin]);
|
||||
yticks([-3*xin, -2*xin, -xin, 0, xin, 2*xin, 3*xin])
|
||||
yticklabels({'', '', '$-\xi\omega_0$', '$0$', ''})
|
||||
|
||||
figure
|
||||
hold on;
|
||||
plot(Wzs, imag(p_ws(1, :)), '-', 'color', colors(1,:))
|
||||
plot(Wzs, imag(p_ws(4, :)), '-', 'color', colors(1,:))
|
||||
plot(Wzs, imag(p_ws(2, :)), '-', 'color', colors(2,:))
|
||||
plot(Wzs, imag(p_ws(3, :)), '-', 'color', colors(2,:))
|
||||
plot(Wzs, zeros(size(Wzs)), 'k--')
|
||||
hold off;
|
||||
xlabel('Rotational Speed $\Omega$'); ylabel('Imaginary Part');
|
||||
xlim([0, 2*w0n]);
|
||||
xticks([0,w0n/2,w0n,3/2*w0n,2*w0n])
|
||||
xticklabels({'$0$', '', '$\omega_0$', '', '$2 \omega_0$'})
|
||||
ylim([-3*w0n, 3*w0n]);
|
||||
yticks([-3*w0n, -2*w0n, -w0n, 0, w0n, 2*w0n, 3*w0n])
|
||||
yticklabels({'', '', '$-\omega_0$', '$0$', '$\omega_0$', '', ''})
|
||||
|
||||
%% Identify the dynamics for several rotating velocities
|
||||
% Sample
|
||||
ms = 0.5; % Sample mass [kg]
|
||||
|
||||
% Tuv Stage
|
||||
kn = 1; % Stiffness [N/m]
|
||||
mn = 0.5; % Tuv mass [kg]
|
||||
cn = 0.01*2*sqrt(kn*(mn+ms)); % Damping [N/(m/s)]
|
||||
|
||||
% General Configuration
|
||||
model_config = struct();
|
||||
model_config.controller = "open_loop"; % Default: Open-Loop
|
||||
model_config.Tuv_type = "normal"; % Default: 2DoF stage
|
||||
|
||||
% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/controller'], 1, 'openinput'); io_i = io_i + 1; % [Fu, Fv]
|
||||
io(io_i) = linio([mdl, '/fd'], 1, 'openinput'); io_i = io_i + 1; % [Fdu, Fdv]
|
||||
io(io_i) = linio([mdl, '/xf'], 1, 'openinput'); io_i = io_i + 1; % [Dfx, Dfy]
|
||||
io(io_i) = linio([mdl, '/translation_stage'], 1, 'openoutput'); io_i = io_i + 1; % [Fmu, Fmv]
|
||||
io(io_i) = linio([mdl, '/translation_stage'], 2, 'openoutput'); io_i = io_i + 1; % [Du, Dv]
|
||||
io(io_i) = linio([mdl, '/ext_metrology'], 1, 'openoutput'); io_i = io_i + 1; % [Dx, Dy]
|
||||
|
||||
% Tested rotating velocities [rad/s]
|
||||
Wzs = [0, 0.1, 0.2, 0.7, 1.2]; % Vector of rotation speeds [rad/s]
|
||||
|
||||
Gs = {zeros(2, 2, length(Wzs))}; % Direct terms
|
||||
|
||||
for i = 1:length(Wzs)
|
||||
Wz = Wzs(i);
|
||||
|
||||
%% Linearize the model
|
||||
G = linearize(mdl, io, 0);
|
||||
|
||||
%% Input/Output definition
|
||||
G.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
|
||||
G.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
Gs{:,:,i} = G;
|
||||
end
|
||||
|
||||
% Save All Identified Plants
|
||||
save('./mat/rotating_generic_plants.mat', 'Gs', 'Wzs');
|
||||
|
||||
%% Bode plot of the direct and coupling terms for several rotating velocities
|
||||
freqs = logspace(-1, 1, 1000);
|
||||
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
% Magnitude
|
||||
ax1 = nexttile([2, 1]);
|
||||
hold on;
|
||||
for i = 1:length(Wzs)
|
||||
plot(freqs, abs(squeeze(freqresp(Gs{i}('du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(i,:))
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]');
|
||||
ylim([1e-2, 1e2]);
|
||||
yticks([1e-2,1e-1,1,1e1,1e2])
|
||||
yticklabels({'$0.01/k$', '$0.1/k$', '$1/k$', '$10/k$', '$100/k$'})
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
for i = 1:length(Wzs)
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{i}('du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(i,:))
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
|
||||
yticks(-180:90:180);
|
||||
ylim([-180 180]);
|
||||
xticks([1e-1,1,1e1])
|
||||
xticklabels({'$0.1 \omega_0$', '$\omega_0$', '$10 \omega_0$'})
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2, 1]);
|
||||
hold on;
|
||||
for i = 1:length(Wzs)
|
||||
plot(freqs, abs(squeeze(freqresp(Gs{i}('dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(i,:), ...
|
||||
'DisplayName', sprintf('$\\Omega = %.1f \\omega_0$', Wzs(i)))
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]');
|
||||
ldg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
||||
ldg.ItemTokenSize = [10, 1];
|
||||
ylim([1e-2, 1e2]);
|
||||
yticks([1e-2,1e-1,1,1e1,1e2])
|
||||
yticklabels({'$0.01/k$', '$0.1/k$', '$1/k$', '$10/k$', '$100/k$'})
|
||||
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
for i = 1:length(Wzs)
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{i}('dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(i,:));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
|
||||
yticks(-180:90:180);
|
||||
ylim([-180 180]);
|
||||
xticks([1e-1,1,1e1])
|
||||
xticklabels({'$0.1 \omega_0$', '$\omega_0$', '$10 \omega_0$'})
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
86
A2-nass-rotating-3dof-model/rotating_2_iff_pure_int.m
Normal file
86
A2-nass-rotating-3dof-model/rotating_2_iff_pure_int.m
Normal file
@ -0,0 +1,86 @@
|
||||
%% Clear Workspace and Close figures
|
||||
clear; close all; clc;
|
||||
|
||||
%% Intialize Laplace variable
|
||||
s = zpk('s');
|
||||
|
||||
%% Path for functions, data and scripts
|
||||
addpath('./mat/'); % Path for data
|
||||
addpath('./src/'); % Path for Functions
|
||||
|
||||
%% Colors for the figures
|
||||
colors = colororder;
|
||||
|
||||
%% Simscape model name
|
||||
mdl = 'rotating_model';
|
||||
|
||||
%% Load "Generic" system dynamics
|
||||
load('rotating_generic_plants.mat', 'Gs', 'Wzs');
|
||||
|
||||
%% Bode plot of the direct and coupling term for Integral Force Feedback - Effect of rotation
|
||||
freqs = logspace(-2, 1, 1000);
|
||||
|
||||
Wz_i = [1,3,4];
|
||||
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
% Magnitude
|
||||
ax1 = nexttile([2, 1]);
|
||||
hold on;
|
||||
for i = 1:length(Wz_i)
|
||||
plot(freqs, abs(squeeze(freqresp(Gs{Wz_i(i)}('fu', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(i,:), ...
|
||||
'DisplayName', sprintf('$\\Omega = %.1f \\omega_0 $', Wzs(Wz_i(i))),'MarkerSize',8);
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]');
|
||||
ylim([1e-3, 1e2]);
|
||||
leg = legend('location', 'northwest', 'FontSize', 8);
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
for i = 1:length(Wz_i)
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{Wz_i(i)}('fu', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(i,:))
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
|
||||
yticks(-180:90:180);
|
||||
ylim([0 180]);
|
||||
xticks([1e-2,1e-1,1,1e1])
|
||||
xticklabels({'$0.01 \omega_0$', '$0.1 \omega_0$', '$\omega_0$', '$10 \omega_0$'})
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
|
||||
%% Root Locus for the Decentralized Integral Force Feedback controller
|
||||
Kiff = 1/s*eye(2);
|
||||
|
||||
gains = logspace(-2, 4, 300);
|
||||
Wz_i = [1,3,4];
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
for i = 1:length(Wz_i)
|
||||
plot(real(pole(Gs{Wz_i(i)}({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), imag(pole(Gs{Wz_i(i)}({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), 'x', 'color', colors(i,:), ...
|
||||
'DisplayName', sprintf('$\\Omega = %.1f \\omega_0 $', Wzs(Wz_i(i))),'MarkerSize',8);
|
||||
plot(real(tzero(Gs{Wz_i(i)}({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), imag(tzero(Gs{Wz_i(i)}({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), 'o', 'color', colors(i,:), ...
|
||||
'HandleVisibility', 'off','MarkerSize',8);
|
||||
for g = gains
|
||||
cl_poles = pole(feedback(Gs{Wz_i(i)}({'fu', 'fv'}, {'Fu', 'Fv'}), g*Kiff, -1));
|
||||
plot(real(cl_poles), imag(cl_poles), '.', 'color', colors(i,:), ...
|
||||
'HandleVisibility', 'off','MarkerSize',4);
|
||||
end
|
||||
end
|
||||
hold off;
|
||||
axis square;
|
||||
xlim([-1.8, 0.2]); ylim([0, 2]);
|
||||
xticks([-1, 0])
|
||||
xticklabels({'-$\omega_0$', '$0$'})
|
||||
yticks([0, 1, 2])
|
||||
yticklabels({'$0$', '$\omega_0$', '$2 \omega_0$'})
|
||||
|
||||
xlabel('Real Part'); ylabel('Imaginary Part');
|
||||
leg = legend('location', 'northwest', 'FontSize', 8);
|
||||
leg.ItemTokenSize(1) = 8;
|
||||
210
A2-nass-rotating-3dof-model/rotating_3_iff_hpf.m
Normal file
210
A2-nass-rotating-3dof-model/rotating_3_iff_hpf.m
Normal file
@ -0,0 +1,210 @@
|
||||
%% Clear Workspace and Close figures
|
||||
clear; close all; clc;
|
||||
|
||||
%% Intialize Laplace variable
|
||||
s = zpk('s');
|
||||
|
||||
%% Path for functions, data and scripts
|
||||
addpath('./mat/'); % Path for data
|
||||
addpath('./src/'); % Path for Functions
|
||||
|
||||
%% Colors for the figures
|
||||
colors = colororder;
|
||||
|
||||
%% Simscape model name
|
||||
mdl = 'rotating_model';
|
||||
|
||||
%% Load "Generic" system dynamics
|
||||
load('rotating_generic_plants.mat', 'Gs', 'Wzs');
|
||||
|
||||
%% Modified IFF - parameters
|
||||
g = 2; % Controller gain
|
||||
wi = 0.1; % HPF Cut-Off frequency [rad/s]
|
||||
|
||||
Kiff = (g/s)*eye(2); % Pure IFF
|
||||
Kiff_hpf = (g/(wi+s))*eye(2); % IFF with added HPF
|
||||
|
||||
%% Loop gain for the IFF with pure integrator and modified IFF with added high-pass filter
|
||||
freqs = logspace(-2, 1, 1000);
|
||||
Wz_i = 2;
|
||||
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
% Magnitude
|
||||
ax1 = nexttile([2, 1]);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(Gs{Wz_i}('fu', 'Fu')*Kiff(1,1), freqs, 'rad/s'))))
|
||||
plot(freqs, abs(squeeze(freqresp(Gs{Wz_i}('fu', 'Fu')*Kiff_hpf(1,1), freqs, 'rad/s'))))
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
set(gca, 'XTickLabel',[]); ylabel('Loop Gain');
|
||||
|
||||
% Phase
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{Wz_i}('fu', 'Fu')*Kiff(1,1), freqs, 'rad/s'))), 'DisplayName', 'IFF')
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{Wz_i}('fu', 'Fu')*Kiff_hpf(1,1), freqs, 'rad/s'))), 'DisplayName', 'IFF,HPF')
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
|
||||
yticks(-180:90:180);
|
||||
ylim([-90 180]);
|
||||
xticks([1e-2,1e-1,1,1e1])
|
||||
xticklabels({'', '$0.1 \omega_0$', '$\omega_0$', '$10 \omega_0$'})
|
||||
leg = legend('location', 'southwest', 'FontSize', 8);
|
||||
leg.ItemTokenSize(1) = 15;
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
|
||||
%% High-Pass Filter Cut-Off Frequency
|
||||
wis = [0.01, 0.05, 0.1]; % [rad/s]
|
||||
|
||||
%% Root Locus for the initial IFF and the modified IFF
|
||||
gains = logspace(-2, 4, 200);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
for wi_i = 1:length(wis)
|
||||
wi = wis(wi_i);
|
||||
Kiff = (1/(wi+s))*eye(2);
|
||||
L(wi_i) = plot(nan, nan, '.', 'color', colors(wi_i,:), 'DisplayName', sprintf('$\\omega_i = %.2f \\omega_0$', wi));
|
||||
for g = gains
|
||||
clpoles = pole(feedback(Gs{2}({'fu', 'fv'}, {'Fu', 'Fv'}), g*Kiff));
|
||||
plot(real(clpoles), imag(clpoles), '.', 'color', colors(wi_i,:),'MarkerSize',4);
|
||||
end
|
||||
plot(real(pole(Gs{2}({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), ...
|
||||
imag(pole(Gs{2}({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), ...
|
||||
'x', 'color', colors(wi_i,:),'MarkerSize',8);
|
||||
plot(real(tzero(Gs{2}({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), ...
|
||||
imag(tzero(Gs{2}({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), ...
|
||||
'o', 'color', colors(wi_i,:),'MarkerSize',8);
|
||||
end
|
||||
hold off;
|
||||
axis equal;
|
||||
xlim([-2.3, 0.1]); ylim([-1.2, 1.2]);
|
||||
xticks([-2:1:2]); yticks([-2:1:2]);
|
||||
leg = legend(L, 'location', 'southwest', 'FontSize', 8);
|
||||
leg.ItemTokenSize(1) = 8;
|
||||
xlabel('Real Part'); ylabel('Imaginary Part');
|
||||
clear L
|
||||
|
||||
xlim([-1.25, 0.25]); ylim([-1.25, 1.25]);
|
||||
xticks([-1, 0])
|
||||
xticklabels({'$-\omega_0$', '$0$'})
|
||||
yticks([-1, 0, 1])
|
||||
yticklabels({'$-\omega_0$', '$0$', '$\omega_0$'})
|
||||
ytickangle(90)
|
||||
|
||||
%% Compute the optimal control gain
|
||||
wis = logspace(-2, 1, 100); % [rad/s]
|
||||
|
||||
opt_xi = zeros(1, length(wis)); % Optimal simultaneous damping
|
||||
opt_gain = zeros(1, length(wis)); % Corresponding optimal gain
|
||||
|
||||
for wi_i = 1:length(wis)
|
||||
wi = wis(wi_i);
|
||||
Kiff = 1/(s + wi)*eye(2);
|
||||
|
||||
fun = @(g)computeSimultaneousDamping(g, Gs{2}({'fu', 'fv'}, {'Fu', 'Fv'}), Kiff);
|
||||
|
||||
[g_opt, xi_opt] = fminsearch(fun, 0.5*wi*((1/Wzs(2))^2 - 1));
|
||||
opt_xi(wi_i) = 1/xi_opt;
|
||||
opt_gain(wi_i) = g_opt;
|
||||
end
|
||||
|
||||
%% Attainable damping ratio as a function of wi/w0. Corresponding control gain g_opt and g_max are also shown
|
||||
figure;
|
||||
yyaxis left
|
||||
plot(wis, opt_xi, '-', 'DisplayName', '$\xi_{cl}$');
|
||||
set(gca, 'YScale', 'lin');
|
||||
ylim([0,1]);
|
||||
ylabel('Damping Ratio $\xi$');
|
||||
|
||||
yyaxis right
|
||||
hold on;
|
||||
plot(wis, opt_gain, '-', 'DisplayName', '$g_{opt}$');
|
||||
plot(wis, wis*((1/Wzs(2))^2 - 1), '--', 'DisplayName', '$g_{max}$');
|
||||
hold off;
|
||||
set(gca, 'YScale', 'lin');
|
||||
ylim([0,10]);
|
||||
ylabel('Controller gain $g$');
|
||||
|
||||
xlabel('$\omega_i/\omega_0$');
|
||||
set(gca, 'XScale', 'log');
|
||||
legend('location', 'northeast', 'FontSize', 8);
|
||||
|
||||
%% Compute damped plant
|
||||
wis = [0.03, 0.1, 0.5]; % [rad/s]
|
||||
g = 2; % Gain
|
||||
|
||||
Gs_iff_hpf = {};
|
||||
|
||||
for i = 1:length(wis)
|
||||
Kiff_hpf = (g/(wis(i)+s))*eye(2); % IFF with added HPF
|
||||
Kiff_hpf.InputName = {'fu', 'fv'};
|
||||
Kiff_hpf.OutputName = {'Fu', 'Fv'};
|
||||
|
||||
Gs_iff_hpf(i) = {feedback(Gs{2}, Kiff_hpf, 'name')};
|
||||
end
|
||||
|
||||
%% Effect of $\omega_i$ on the damped plant coupling
|
||||
freqs = logspace(-2, 1, 1000);
|
||||
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
% Magnitude
|
||||
ax1 = nexttile([2, 1]);
|
||||
hold on;
|
||||
for i = 1:length(wis)
|
||||
plot(freqs, abs(squeeze(freqresp(Gs_iff_hpf{i}('Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', [colors(i,:)], ...
|
||||
'DisplayName', sprintf('$d_u/F_u$, $\\omega_i = %.2f \\omega_0$', wis(i)))
|
||||
plot(freqs, abs(squeeze(freqresp(Gs_iff_hpf{i}('Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [colors(i,:), 0.5], ...
|
||||
'DisplayName', sprintf('$d_v/F_u$, $\\omega_i = %.2f \\omega_0$', wis(i)))
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]');
|
||||
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
||||
leg.ItemTokenSize(1) = 20;
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
for i = 1:length(wis)
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs_iff_hpf{i}('Du', 'Fu'), freqs, 'rad/s'))), '-')
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
|
||||
yticks(-180:90:180);
|
||||
ylim([-180 0]);
|
||||
xticks([1e-2,1e-1,1,1e1])
|
||||
xticklabels({'$0.01 \omega_0$', '$0.1 \omega_0$', '$\omega_0$', '$10 \omega_0$'})
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
|
||||
%% Effect of $\omega_i$ on the obtained compliance
|
||||
freqs = logspace(-2, 1, 1000);
|
||||
|
||||
figure;
|
||||
tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
% Magnitude
|
||||
ax1 = nexttile();
|
||||
hold on;
|
||||
for i = 1:length(wis)
|
||||
plot(freqs, abs(squeeze(freqresp(Gs_iff_hpf{i}('Du', 'Fdx'), freqs, 'rad/s'))), '-', 'color', [colors(i,:)], ...
|
||||
'DisplayName', sprintf('$d_{x}/F_{dx}$, $\\omega_i = %.2f \\omega_0$', wis(i)))
|
||||
end
|
||||
plot(freqs, abs(squeeze(freqresp(Gs{2}('Du', 'Fdx'), freqs, 'rad/s'))), 'k--', ...
|
||||
'DisplayName', '$d_{x}/F_{dx}$, OL')
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [rad/s]'); ylabel('Compliance [m/N]');
|
||||
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
||||
leg.ItemTokenSize(1) = 20;
|
||||
xticks([1e-2,1e-1,1,1e1])
|
||||
xticklabels({'$0.01 \omega_0$', '$0.1 \omega_0$', '$\omega_0$', '$10 \omega_0$'})
|
||||
339
A2-nass-rotating-3dof-model/rotating_4_iff_kp.m
Normal file
339
A2-nass-rotating-3dof-model/rotating_4_iff_kp.m
Normal file
@ -0,0 +1,339 @@
|
||||
%% Clear Workspace and Close figures
|
||||
clear; close all; clc;
|
||||
|
||||
%% Intialize Laplace variable
|
||||
s = zpk('s');
|
||||
|
||||
%% Path for functions, data and scripts
|
||||
addpath('./mat/'); % Path for data
|
||||
addpath('./src/'); % Path for Functions
|
||||
|
||||
%% Colors for the figures
|
||||
colors = colororder;
|
||||
|
||||
%% Simscape model name
|
||||
mdl = 'rotating_model';
|
||||
|
||||
%% Load "Generic" system dynamics
|
||||
load('rotating_generic_plants.mat', 'Gs', 'Wzs');
|
||||
|
||||
%% Tuv Stage
|
||||
mn = 0.5; % Tuv mass [kg]
|
||||
|
||||
%% Sample
|
||||
ms = 0.5; % Sample mass [kg]
|
||||
|
||||
%% General Configuration
|
||||
model_config = struct();
|
||||
model_config.controller = "open_loop"; % Default: Open-Loop
|
||||
model_config.Tuv_type = "parallel_k"; % Default: 2DoF stage
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/controller'], 1, 'openinput'); io_i = io_i + 1; % [Fu, Fv]
|
||||
io(io_i) = linio([mdl, '/fd'], 1, 'openinput'); io_i = io_i + 1; % [Fdu, Fdv]
|
||||
io(io_i) = linio([mdl, '/translation_stage'], 1, 'openoutput'); io_i = io_i + 1; % [Fmu, Fmv]
|
||||
io(io_i) = linio([mdl, '/translation_stage'], 2, 'openoutput'); io_i = io_i + 1; % [Du, Dv]
|
||||
io(io_i) = linio([mdl, '/ext_metrology'], 1, 'openoutput'); io_i = io_i + 1; % [Dx, Dy]
|
||||
|
||||
Wz = 0.1; % The rotation speed [rad/s]
|
||||
|
||||
%% No parallel Stiffness
|
||||
kp = 0; % Parallel Stiffness [N/m]
|
||||
cp = 0.001*2*sqrt(kp*(mn+ms)); % Small parallel damping [N/(m/s)]
|
||||
kn = 1 - kp; % Stiffness [N/m]
|
||||
cn = 0.01*2*sqrt(kn*(mn+ms)); % Damping [N/(m/s)]
|
||||
|
||||
G_no_kp = linearize(mdl, io, 0);
|
||||
G_no_kp.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy'};
|
||||
G_no_kp.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
%% Small parallel Stiffness
|
||||
kp = 0.5*(mn+ms)*Wz^2; % Parallel Stiffness [N/m]
|
||||
cp = 0.001*2*sqrt(kp*(mn+ms)); % Small parallel damping [N/(m/s)]
|
||||
kn = 1 - kp; % Stiffness [N/m]
|
||||
cn = 0.01*2*sqrt(kn*(mn+ms)); % Damping [N/(m/s)]
|
||||
|
||||
G_low_kp = linearize(mdl, io, 0);
|
||||
G_low_kp.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy'};
|
||||
G_low_kp.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
%% Large parallel Stiffness
|
||||
kp = 1.5*(mn+ms)*Wz^2; % Parallel Stiffness [N/m]
|
||||
cp = 0.001*2*sqrt(kp*(mn+ms)); % Small parallel damping [N/(m/s)]
|
||||
kn = 1 - kp; % Stiffness [N/m]
|
||||
cn = 0.01*2*sqrt(kn*(mn+ms)); % Damping [N/(m/s)]
|
||||
|
||||
G_high_kp = linearize(mdl, io, 0);
|
||||
G_high_kp.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy'};
|
||||
G_high_kp.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
%% Effect of the parallel stiffness on the IFF plant
|
||||
freqs = logspace(-2, 1, 1000);
|
||||
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
% Magnitude
|
||||
ax1 = nexttile([2, 1]);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_no_kp( 'fu', 'Fu'), freqs, 'rad/s'))), '-', ...
|
||||
'DisplayName', '$k_p = 0$')
|
||||
plot(freqs, abs(squeeze(freqresp(G_low_kp( 'fu', 'Fu'), freqs, 'rad/s'))), '-', ...
|
||||
'DisplayName', '$k_p < m\Omega^2$')
|
||||
plot(freqs, abs(squeeze(freqresp(G_high_kp('fu', 'Fu'), freqs, 'rad/s'))), '-', ...
|
||||
'DisplayName', '$k_p > m\Omega^2$')
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]');
|
||||
ylim([1e-4, 5e1]);
|
||||
legend('location', 'southeast', 'FontSize', 8);
|
||||
|
||||
% Phase
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_no_kp( 'fu', 'Fu'), freqs, 'rad/s'))), '-')
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_low_kp( 'fu', 'Fu'), freqs, 'rad/s'))), '-')
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_high_kp('fu', 'Fu'), freqs, 'rad/s'))), '-')
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
|
||||
yticks(-180:90:180);
|
||||
ylim([0 180]);
|
||||
hold off;
|
||||
xticks([1e-2,1e-1,1,1e1])
|
||||
xticklabels({'$0.01 \omega_0$', '$0.1 \omega_0$', '$\omega_0$', '$10 \omega_0$'})
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
|
||||
%% Root Locus for IFF without parallel spring, with small parallel spring and with large parallel spring
|
||||
gains = logspace(-2, 2, 200);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(real(pole(G_no_kp({'fu','fv'},{'Fu','Fv'})*(1/s))), imag(pole(G_no_kp({'fu','fv'},{'Fu','Fv'})*(1/s))), 'x', 'color', colors(1,:), ...
|
||||
'DisplayName', '$k_p = 0$','MarkerSize',8);
|
||||
plot(real(tzero(G_no_kp({'fu','fv'},{'Fu','Fv'})*(1/s))), imag(tzero(G_no_kp({'fu','fv'},{'Fu','Fv'})*(1/s))), 'o', 'color', colors(1,:), ...
|
||||
'HandleVisibility', 'off','MarkerSize',8);
|
||||
for g = gains
|
||||
cl_poles = pole(feedback(G_no_kp({'fu','fv'},{'Fu','Fv'}), (g/s)*eye(2)));
|
||||
plot(real(cl_poles), imag(cl_poles), '.', 'color', colors(1,:), ...
|
||||
'HandleVisibility', 'off','MarkerSize',4);
|
||||
end
|
||||
|
||||
plot(real(pole(G_low_kp({'fu','fv'},{'Fu','Fv'})*(1/s))), imag(pole(G_low_kp({'fu','fv'},{'Fu','Fv'})*(1/s))), 'x', 'color', colors(2,:), ...
|
||||
'DisplayName', '$k_p < m\Omega^2$','MarkerSize',8);
|
||||
plot(real(tzero(G_low_kp({'fu','fv'},{'Fu','Fv'})*(1/s))), imag(tzero(G_low_kp({'fu','fv'},{'Fu','Fv'})*(1/s))), 'o', 'color', colors(2,:), ...
|
||||
'HandleVisibility', 'off','MarkerSize',8);
|
||||
for g = gains
|
||||
cl_poles = pole(feedback(G_low_kp({'fu','fv'},{'Fu','Fv'}), (g/s)*eye(2)));
|
||||
plot(real(cl_poles), imag(cl_poles), '.', 'color', colors(2,:), ...
|
||||
'HandleVisibility', 'off','MarkerSize',4);
|
||||
end
|
||||
|
||||
plot(real(pole(G_high_kp({'fu','fv'},{'Fu','Fv'})*(1/s))), imag(pole(G_high_kp({'fu','fv'},{'Fu','Fv'})*(1/s))), 'x', 'color', colors(3,:), ...
|
||||
'DisplayName', '$k_p > m\Omega^2$','MarkerSize',8);
|
||||
plot(real(tzero(G_high_kp({'fu','fv'},{'Fu','Fv'})*(1/s))), imag(tzero(G_high_kp({'fu','fv'},{'Fu','Fv'})*(1/s))), 'o', 'color', colors(3,:), ...
|
||||
'HandleVisibility', 'off','MarkerSize',8);
|
||||
for g = gains
|
||||
cl_poles = pole(feedback(G_high_kp({'fu','fv'},{'Fu','Fv'}), (g/s)*eye(2)));
|
||||
plot(real(cl_poles), imag(cl_poles), '.', 'color', colors(3,:), ...
|
||||
'HandleVisibility', 'off','MarkerSize',4);
|
||||
end
|
||||
hold off;
|
||||
axis square;
|
||||
xlim([-2.25, 0.25]); ylim([-1.25, 1.25]);
|
||||
xticks([-2, -1, 0])
|
||||
xticklabels({'$-2\omega_0$', '$-\omega_0$', '$0$'})
|
||||
yticks([-1, 0, 1])
|
||||
yticklabels({'$-\omega_0$', '$0$', '$\omega_0$'})
|
||||
|
||||
xlabel('Real Part'); ylabel('Imaginary Part');
|
||||
leg = legend('location', 'northwest', 'FontSize', 8);
|
||||
leg.ItemTokenSize(1) = 8;
|
||||
|
||||
%% Tested parallel stiffnesses
|
||||
kps = [2, 20, 40]*(mn + ms)*Wz^2;
|
||||
|
||||
%% Root Locus: Effect of the parallel stiffness on the attainable damping
|
||||
gains = logspace(-2, 4, 500);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
for kp_i = 1:length(kps)
|
||||
kp = kps(kp_i); % Parallel Stiffness [N/m]
|
||||
cp = 0.001*2*sqrt(kp*(mn+ms)); % Small parallel damping [N/(m/s)]
|
||||
kn = 1 - kp; % Stiffness [N/m]
|
||||
cn = 0.01*2*sqrt(kn*(mn+ms)); % Damping [N/(m/s)]
|
||||
|
||||
% Identify dynamics
|
||||
G = linearize(mdl, io, 0);
|
||||
G.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy'};
|
||||
G.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
plot(real(pole(G({'fu', 'fv'}, {'Fu', 'Fv'})*(1/s*eye(2)))), imag(pole(G({'fu', 'fv'}, {'Fu', 'Fv'})*(1/s*eye(2)))), 'x', 'color', colors(kp_i,:), ...
|
||||
'DisplayName', sprintf('$k_p = %.1f m \\Omega^2$', kp/((mn+ms)*Wz^2)),'MarkerSize',8);
|
||||
plot(real(tzero(G({'fu', 'fv'}, {'Fu', 'Fv'})*(1/s*eye(2)))), imag(tzero(G({'fu', 'fv'}, {'Fu', 'Fv'})*(1/s*eye(2)))), 'o', 'color', colors(kp_i,:), ...
|
||||
'HandleVisibility', 'off','MarkerSize',8);
|
||||
for g = gains
|
||||
cl_poles = pole(feedback(G({'fu', 'fv'}, {'Fu', 'Fv'}), (g/s)*eye(2)));
|
||||
plot(real(cl_poles), imag(cl_poles), '.', 'color', colors(kp_i,:),'MarkerSize',4, ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
end
|
||||
hold off;
|
||||
axis square;
|
||||
% xlim([-1.15, 0.05]); ylim([0, 1.2]);
|
||||
xlim([-2.25, 0.25]); ylim([-1.25, 1.25]);
|
||||
xticks([-2, -1, 0])
|
||||
xticklabels({'$-2\omega_0$', '$-\omega_0$', '$0$'})
|
||||
yticks([-1, 0, 1])
|
||||
yticklabels({'$-\omega_0$', '$0$', '$\omega_0$'})
|
||||
|
||||
xlabel('Real Part'); ylabel('Imaginary Part');
|
||||
leg = legend('location', 'northwest', 'FontSize', 8);
|
||||
leg.ItemTokenSize(1) = 12;
|
||||
|
||||
%% Computes the optimal parameters and attainable simultaneous damping
|
||||
alphas = logspace(-2, 0, 100);
|
||||
alphas(end) = []; % Remove last point
|
||||
|
||||
opt_xi = zeros(1, length(alphas)); % Optimal simultaneous damping
|
||||
opt_gain = zeros(1, length(alphas)); % Corresponding optimal gain
|
||||
|
||||
Kiff = 1/s*eye(2);
|
||||
|
||||
for alpha_i = 1:length(alphas)
|
||||
kp = alphas(alpha_i);
|
||||
cp = 0.001*2*sqrt(kp*(mn+ms)); % Small parallel damping [N/(m/s)]
|
||||
kn = 1 - kp; % Stiffness [N/m]
|
||||
cn = 0.01*2*sqrt(kn*(mn+ms)); % Damping [N/(m/s)]
|
||||
|
||||
% Identify dynamics
|
||||
G = linearize(mdl, io, 0);
|
||||
G.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy'};
|
||||
G.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
fun = @(g)computeSimultaneousDamping(g, G({'fu', 'fv'}, {'Fu', 'Fv'}), Kiff);
|
||||
|
||||
[g_opt, xi_opt] = fminsearch(fun, 2);
|
||||
opt_xi(alpha_i) = 1/xi_opt;
|
||||
opt_gain(alpha_i) = g_opt;
|
||||
end
|
||||
|
||||
%% Attainable damping as a function of the stiffness ratio
|
||||
figure;
|
||||
yyaxis left
|
||||
plot(alphas, opt_xi, '-', 'DisplayName', '$\xi_{cl}$');
|
||||
set(gca, 'YScale', 'lin');
|
||||
ylim([0,1]);
|
||||
ylabel('Damping Ratio $\xi$');
|
||||
|
||||
yyaxis right
|
||||
hold on;
|
||||
plot(alphas, opt_gain, '-', 'DisplayName', '$g_{opt}$');
|
||||
set(gca, 'YScale', 'lin');
|
||||
ylim([0,2.5]);
|
||||
ylabel('Controller gain $g$');
|
||||
|
||||
set(gca, 'XScale', 'log');
|
||||
legend('location', 'northeast', 'FontSize', 8);
|
||||
|
||||
xlabel('$k_p$');
|
||||
xlim([0.01, 1]);
|
||||
xticks([0.01, 0.1, 1])
|
||||
xticklabels({'$m\Omega^2$', '$10m\Omega^2$', '$100m\Omega^2$'})
|
||||
|
||||
%% Identify dynamics with parallel stiffness = 2mW^2
|
||||
Wz = 0.1; % [rad/s]
|
||||
kp = 2*(mn + ms)*Wz^2; % Parallel Stiffness [N/m]
|
||||
cp = 0.001*2*sqrt(kp*(mn+ms)); % Small parallel damping [N/(m/s)]
|
||||
kn = 1 - kp; % Stiffness [N/m]
|
||||
cn = 0.01*2*sqrt(kn*(mn+ms)); % Damping [N/(m/s)]
|
||||
|
||||
% Identify dynamics
|
||||
G = linearize(mdl, io, 0);
|
||||
G.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy'};
|
||||
G.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
%% IFF controller with pure integrator
|
||||
Kiff_kp = (2.2/s)*eye(2);
|
||||
Kiff_kp.InputName = {'fu', 'fv'};
|
||||
Kiff_kp.OutputName = {'Fu', 'Fv'};
|
||||
|
||||
%% Compute the damped plant
|
||||
G_cl_iff_kp = feedback(G, Kiff_kp, 'name');
|
||||
|
||||
w0 = sqrt((kn+kp)/(mn+ms)); % Resonance frequency [rad/s]
|
||||
wis = w0*logspace(-2, 0, 100); % LPF cut-off [rad/s]
|
||||
|
||||
%% Computes the obtained damping as a function of the HPF cut-off frequency
|
||||
opt_xi = zeros(1, length(wis)); % Optimal simultaneous damping
|
||||
|
||||
for wi_i = 1:length(wis)
|
||||
Kiff_kp_hpf = (2.2/(s + wis(wi_i)))*eye(2);
|
||||
Kiff_kp_hpf.InputName = {'fu', 'fv'};
|
||||
Kiff_kp_hpf.OutputName = {'Fu', 'Fv'};
|
||||
|
||||
[~, xi] = damp(feedback(G, Kiff_kp_hpf, 'name'));
|
||||
opt_xi(wi_i) = min(xi);
|
||||
end
|
||||
|
||||
%% Effect of the high-pass filter cut-off frequency on the obtained damping
|
||||
figure;
|
||||
plot(wis, opt_xi, '-');
|
||||
set(gca, 'XScale', 'log');
|
||||
set(gca, 'YScale', 'lin');
|
||||
ylim([0,1]);
|
||||
ylabel('Damping Ratio $\xi$');
|
||||
xlabel('$\omega_i/\omega_0$');
|
||||
|
||||
%% Compute the damped plant with added High-Pass Filter
|
||||
Kiff_kp_hpf = (2.2/(s + 0.1*w0))*eye(2);
|
||||
Kiff_kp_hpf.InputName = {'fu', 'fv'};
|
||||
Kiff_kp_hpf.OutputName = {'Fu', 'Fv'};
|
||||
|
||||
G_cl_iff_hpf_kp = feedback(G, Kiff_kp_hpf, 'name');
|
||||
|
||||
%% Bode plot of the direct and coupling terms for several rotating velocities
|
||||
freqs = logspace(-3, 1, 1000);
|
||||
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
% Magnitude
|
||||
ax1 = nexttile([2, 1]);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G( 'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', zeros( 1,3), ...
|
||||
'DisplayName', '$d_u/F_u$ - OL')
|
||||
plot(freqs, abs(squeeze(freqresp(G_cl_iff_kp( 'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(1,:), ...
|
||||
'DisplayName', '$d_u/F_u$ - IFF + $k_p$')
|
||||
plot(freqs, abs(squeeze(freqresp(G_cl_iff_hpf_kp('Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(2,:), ...
|
||||
'DisplayName', '$d_u/F_u$ - IFF + $k_p$ + HPF')
|
||||
plot(freqs, abs(squeeze(freqresp(G( 'Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [zeros( 1,3), 0.5], ...
|
||||
'HandleVisibility', 'off')
|
||||
plot(freqs, abs(squeeze(freqresp(G_cl_iff_kp( 'Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [colors(1,:), 0.5], ...
|
||||
'HandleVisibility', 'off')
|
||||
plot(freqs, abs(squeeze(freqresp(G_cl_iff_hpf_kp('Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [colors(2,:), 0.5], ...
|
||||
'HandleVisibility', 'off')
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]');
|
||||
ldg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
||||
ldg.ItemTokenSize(1) = 10;
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G( 'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', zeros( 1,3))
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cl_iff_kp( 'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(1,:))
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cl_iff_hpf_kp('Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(2,:))
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
|
||||
yticks(-180:90:180);
|
||||
ylim([-180 90]);
|
||||
xticks([1e-3,1e-2,1e-1,1,1e1])
|
||||
xticklabels({'$0.001 \omega_0$', '$0.01 \omega_0$', '$0.1 \omega_0$', '$\omega_0$', '$10 \omega_0$'})
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
96
A2-nass-rotating-3dof-model/rotating_5_rdc.m
Normal file
96
A2-nass-rotating-3dof-model/rotating_5_rdc.m
Normal file
@ -0,0 +1,96 @@
|
||||
%% Clear Workspace and Close figures
|
||||
clear; close all; clc;
|
||||
|
||||
%% Intialize Laplace variable
|
||||
s = zpk('s');
|
||||
|
||||
%% Path for functions, data and scripts
|
||||
addpath('./mat/'); % Path for data
|
||||
addpath('./src/'); % Path for Functions
|
||||
|
||||
%% Colors for the figures
|
||||
colors = colororder;
|
||||
|
||||
%% Simscape model name
|
||||
mdl = 'rotating_model';
|
||||
|
||||
%% Load "Generic" system dynamics
|
||||
load('rotating_generic_plants.mat', 'Gs', 'Wzs');
|
||||
|
||||
%% Root Locus for Relative Damping Control
|
||||
Krdc = s*eye(2); % Relative damping controller
|
||||
|
||||
gains = logspace(-2, 2, 300); % Tested gains
|
||||
Wz_i = [1,3,4];
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
for i = 1:length(Wz_i)
|
||||
plot(real(pole(Gs{Wz_i(i)}({'du', 'dv'}, {'Fu', 'Fv'})*Krdc)), imag(pole(Gs{Wz_i(i)}({'du', 'dv'}, {'Fu', 'Fv'})*Krdc)), 'x', 'color', colors(i,:), ...
|
||||
'DisplayName', sprintf('$\\Omega = %.1f \\omega_0 $', Wzs(Wz_i(i))),'MarkerSize',8);
|
||||
plot(real(tzero(Gs{Wz_i(i)}({'du', 'dv'}, {'Fu', 'Fv'})*Krdc)), imag(tzero(Gs{Wz_i(i)}({'du', 'dv'}, {'Fu', 'Fv'})*Krdc)), 'o', 'color', colors(i,:), ...
|
||||
'HandleVisibility', 'off','MarkerSize',8);
|
||||
for g = gains
|
||||
cl_poles = pole(feedback(Gs{Wz_i(i)}({'du', 'dv'}, {'Fu', 'Fv'}), g*Krdc, -1));
|
||||
plot(real(cl_poles), imag(cl_poles), '.', 'color', colors(i,:), ...
|
||||
'HandleVisibility', 'off','MarkerSize',4);
|
||||
end
|
||||
end
|
||||
hold off;
|
||||
axis square;
|
||||
xlim([-1.8, 0.2]); ylim([0, 2]);
|
||||
xticks([-1, 0])
|
||||
xticklabels({'-$\omega_0$', '$0$'})
|
||||
yticks([0, 1, 2])
|
||||
yticklabels({'$0$', '$\omega_0$', '$2 \omega_0$'})
|
||||
|
||||
xlabel('Real Part'); ylabel('Imaginary Part');
|
||||
leg = legend('location', 'northwest', 'FontSize', 8);
|
||||
leg.ItemTokenSize(1) = 8;
|
||||
|
||||
i = 2;
|
||||
|
||||
%% Relative Damping Controller
|
||||
Krdc = 2*s*eye(2);
|
||||
Krdc.InputName = {'Du', 'Dv'};
|
||||
Krdc.OutputName = {'Fu', 'Fv'};
|
||||
|
||||
%% Compute the damped plant
|
||||
G_cl_rdc = feedback(Gs{i}, Krdc, 'name');
|
||||
|
||||
%% Damped plant using Relative Damping Control
|
||||
freqs = logspace(-3, 2, 1000);
|
||||
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
% Magnitude
|
||||
ax1 = nexttile([2, 1]);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(Gs{i}( 'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', zeros(1,3), ...
|
||||
'DisplayName', 'OL - $G_d(1,1)$')
|
||||
plot(freqs, abs(squeeze(freqresp(G_cl_rdc('Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(1,:), ...
|
||||
'DisplayName', 'RDC - $G_d(1,1)$')
|
||||
plot(freqs, abs(squeeze(freqresp(Gs{i}( 'Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [zeros(1,3), 0.5], ...
|
||||
'DisplayName', 'OL - $G_d(2,1)$')
|
||||
plot(freqs, abs(squeeze(freqresp(G_cl_rdc('Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [colors(1,:), 0.5], ...
|
||||
'DisplayName', 'RDC - $G_d(2,1)$')
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]');
|
||||
ldg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
|
||||
ldg.ItemTokenSize(1) = 20;
|
||||
ylim([1e-4, 1e2]);
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{i}('Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', zeros(1,3))
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cl_rdc('Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(1,:))
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
|
||||
yticks(-180:90:180);
|
||||
ylim([-180 0]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
213
A2-nass-rotating-3dof-model/rotating_6_act_damp_comparison.m
Normal file
213
A2-nass-rotating-3dof-model/rotating_6_act_damp_comparison.m
Normal file
@ -0,0 +1,213 @@
|
||||
%% Clear Workspace and Close figures
|
||||
clear; close all; clc;
|
||||
|
||||
%% Intialize Laplace variable
|
||||
s = zpk('s');
|
||||
|
||||
%% Path for functions, data and scripts
|
||||
addpath('./mat/'); % Path for data
|
||||
addpath('./src/'); % Path for Functions
|
||||
|
||||
%% Colors for the figures
|
||||
colors = colororder;
|
||||
|
||||
%% Simscape model name
|
||||
mdl = 'rotating_model';
|
||||
|
||||
%% The rotating speed is set to $\Omega = 0.1 \omega_0$.
|
||||
Wz = 0.1; % [rad/s]
|
||||
|
||||
%% Masses
|
||||
ms = 0.5; % Sample mass [kg]
|
||||
mn = 0.5; % Tuv mass [kg]
|
||||
|
||||
%% General Configuration
|
||||
model_config = struct();
|
||||
model_config.controller = "open_loop"; % Default: Open-Loop
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/controller'], 1, 'openinput'); io_i = io_i + 1; % [Fu, Fv]
|
||||
io(io_i) = linio([mdl, '/fd'], 1, 'openinput'); io_i = io_i + 1; % [Fdu, Fdv]
|
||||
io(io_i) = linio([mdl, '/xf'], 1, 'openinput'); io_i = io_i + 1; % [Dfx, Dfy]
|
||||
io(io_i) = linio([mdl, '/translation_stage'], 1, 'openoutput'); io_i = io_i + 1; % [Fmu, Fmv]
|
||||
io(io_i) = linio([mdl, '/translation_stage'], 2, 'openoutput'); io_i = io_i + 1; % [Du, Dv]
|
||||
io(io_i) = linio([mdl, '/ext_metrology'], 1, 'openoutput'); io_i = io_i + 1; % [Dx, Dy]
|
||||
|
||||
%% Identifying plant with parallel stiffness
|
||||
model_config.Tuv_type = "parallel_k";
|
||||
|
||||
% Parallel stiffness
|
||||
kp = 2*(mn+ms)*Wz^2; % Parallel Stiffness [N/m]
|
||||
cp = 0.001*2*sqrt(kp*(mn+ms)); % Small parallel damping [N/(m/s)]
|
||||
|
||||
% Tuv Stage
|
||||
kn = 1 - kp; % Stiffness [N/m]
|
||||
cn = 0.01*2*sqrt(kn*(mn+ms)); % Damping [N/(m/s)]
|
||||
|
||||
% Linearize
|
||||
G_kp = linearize(mdl, io, 0);
|
||||
G_kp.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
|
||||
G_kp.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
%% Identifying plant with no parallel stiffness
|
||||
model_config.Tuv_type = "normal";
|
||||
|
||||
% Tuv Stage
|
||||
kn = 1; % Stiffness [N/m]
|
||||
cn = 0.01*2*sqrt(kn*(mn+ms)); % Damping [N/(m/s)]
|
||||
|
||||
% Linearize
|
||||
G = linearize(mdl, io, 0);
|
||||
G.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
|
||||
G.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
%% IFF Controller
|
||||
Kiff = (2.2/(s + 0.1))*eye(2);
|
||||
Kiff.InputName = {'fu', 'fv'};
|
||||
Kiff.OutputName = {'Fu', 'Fv'};
|
||||
|
||||
%% IFF Controller with added stiffness
|
||||
Kiff_kp = (2.2/(s + 0.1))*eye(2);
|
||||
Kiff_kp.InputName = {'fu', 'fv'};
|
||||
Kiff_kp.OutputName = {'Fu', 'Fv'};
|
||||
|
||||
%% Relative Damping Controller
|
||||
Krdc = 2*s*eye(2);
|
||||
Krdc.InputName = {'Du', 'Dv'};
|
||||
Krdc.OutputName = {'Fu', 'Fv'};
|
||||
|
||||
%% Comparison of active damping techniques for rotating platform - Root Locus
|
||||
gains = logspace(-2, 2, 500);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
% IFF
|
||||
plot(real(pole(G({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), imag(pole(G({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), 'x', 'color', colors(1,:), ...
|
||||
'DisplayName', 'IFF with HPF', 'MarkerSize', 8);
|
||||
plot(real(tzero(G({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), imag(tzero(G({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), 'o', 'color', colors(1,:), ...
|
||||
'HandleVisibility', 'off', 'MarkerSize', 8);
|
||||
for g = gains
|
||||
cl_poles = pole(feedback(G({'fu', 'fv'}, {'Fu', 'Fv'}), g*Kiff));
|
||||
plot(real(cl_poles), imag(cl_poles), '.', 'color', colors(1,:),'MarkerSize',4, ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
% IFF with parallel stiffness
|
||||
plot(real(pole(G_kp({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff_kp)), imag(pole(G_kp({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff_kp)), 'x', 'color', colors(2,:), ...
|
||||
'DisplayName', 'IFF with $k_p$', 'MarkerSize', 8);
|
||||
plot(real(tzero(G_kp({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff_kp)), imag(tzero(G_kp({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff_kp)), 'o', 'color', colors(2,:), ...
|
||||
'HandleVisibility', 'off', 'MarkerSize', 8);
|
||||
for g = gains
|
||||
cl_poles = pole(feedback(G_kp({'fu', 'fv'}, {'Fu', 'Fv'}), g*Kiff_kp));
|
||||
plot(real(cl_poles), imag(cl_poles), '.', 'color', colors(2,:),'MarkerSize',4, ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
% RDC
|
||||
plot(real(pole(G({'Du', 'Dv'}, {'Fu', 'Fv'})*Krdc)), imag(pole(G({'Du', 'Dv'}, {'Fu', 'Fv'})*Krdc)), 'x', 'color', colors(3,:), ...
|
||||
'DisplayName', 'RDC', 'MarkerSize', 8);
|
||||
plot(real(tzero(G({'Du', 'Dv'}, {'Fu', 'Fv'})*Krdc)), imag(tzero(G({'Du', 'Dv'}, {'Fu', 'Fv'})*Krdc)), 'o', 'color', colors(3,:), ...
|
||||
'HandleVisibility', 'off', 'MarkerSize', 8);
|
||||
for g = gains
|
||||
cl_poles = pole(feedback(G({'Du', 'Dv'}, {'Fu', 'Fv'}), g*Krdc));
|
||||
plot(real(cl_poles), imag(cl_poles), '.', 'color', colors(3,:),'MarkerSize',4, ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
hold off;
|
||||
axis square;
|
||||
xlim([-1.15, 0.05]); ylim([0, 1.2]);
|
||||
|
||||
xlabel('Real Part'); ylabel('Imaginary Part');
|
||||
leg = legend('location', 'northwest', 'FontSize', 8);
|
||||
leg.ItemTokenSize(1) = 12;
|
||||
|
||||
%% Compute Damped plants
|
||||
G_cl_iff = feedback(G, Kiff, 'name');
|
||||
G_cl_iff_kp = feedback(G_kp, Kiff_kp, 'name');
|
||||
G_cl_rdc = feedback(G, Krdc, 'name');
|
||||
|
||||
%% Comparison of the damped plants obtained with the three active damping techniques
|
||||
freqs = logspace(-2, 2, 1000);
|
||||
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
% Magnitude
|
||||
ax1 = nexttile([2, 1]);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G( 'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', zeros(1,3), ...
|
||||
'DisplayName', 'OL')
|
||||
plot(freqs, abs(squeeze(freqresp(G_cl_iff( 'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(1,:), ...
|
||||
'DisplayName', 'IFF + HPF')
|
||||
plot(freqs, abs(squeeze(freqresp(G_cl_iff_kp('Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(2,:), ...
|
||||
'DisplayName', 'IFF + $k_p$')
|
||||
plot(freqs, abs(squeeze(freqresp(G_cl_rdc( 'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(3,:), ...
|
||||
'DisplayName', 'RDC')
|
||||
plot(freqs, abs(squeeze(freqresp(G( 'Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [zeros(1,3), 0.5], ...
|
||||
'DisplayName', 'Coupling')
|
||||
plot(freqs, abs(squeeze(freqresp(G_cl_iff( 'Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [colors(1,:), 0.5], ...
|
||||
'DisplayName', 'Coupling')
|
||||
plot(freqs, abs(squeeze(freqresp(G_cl_iff_kp('Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [colors(2,:), 0.5], ...
|
||||
'DisplayName', 'Coupling')
|
||||
plot(freqs, abs(squeeze(freqresp(G_cl_rdc( 'Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [colors(3,:), 0.5], ...
|
||||
'DisplayName', 'Coupling')
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]');
|
||||
ldg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
|
||||
ldg.ItemTokenSize = [10, 1];
|
||||
ylim([1e-6, 1e2])
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G( 'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', zeros(1,3))
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cl_iff( 'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(1,:))
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cl_iff_kp('Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(2,:))
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cl_rdc( 'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(3,:))
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
|
||||
yticks(-180:90:180);
|
||||
ylim([-180 15]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
xticks([1e-2,1e-1,1,1e1,1e2])
|
||||
xticklabels({'$0.01 \omega_0$', '$0.1 \omega_0$', '$\omega_0$', '$10 \omega_0$', '$100 \omega_0$'})
|
||||
|
||||
%% Comparison of the obtained transmissibility and compliance for the three tested active damping techniques
|
||||
freqs = logspace(-2, 2, 1000);
|
||||
|
||||
% transmissibility
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G( 'Dx', 'Dfx'), freqs, 'rad/s'))), '-', 'color', zeros(1,3), ...
|
||||
'DisplayName', 'OL')
|
||||
plot(freqs, abs(squeeze(freqresp(G_cl_iff( 'Dx', 'Dfx'), freqs, 'rad/s'))), '-', 'color', colors(1,:), ...
|
||||
'DisplayName', 'IFF + HPF')
|
||||
plot(freqs, abs(squeeze(freqresp(G_cl_iff_kp('Dx', 'Dfx'), freqs, 'rad/s'))), '-', 'color', colors(2,:), ...
|
||||
'DisplayName', 'IFF + $k_p$')
|
||||
plot(freqs, abs(squeeze(freqresp(G_cl_rdc( 'Dx', 'Dfx'), freqs, 'rad/s'))), '-', 'color', colors(3,:), ...
|
||||
'DisplayName', 'RDC')
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [rad/s]'); ylabel('Transmissibility [m/m]');
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
|
||||
% Compliance
|
||||
figure;
|
||||
ax1 = nexttile();
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G( 'Dx', 'Fdx'), freqs, 'rad/s'))), '-', 'color', zeros(1,3), ...
|
||||
'DisplayName', 'OL')
|
||||
plot(freqs, abs(squeeze(freqresp(G_cl_iff( 'Dx', 'Fdx'), freqs, 'rad/s'))), '-', 'color', colors(1,:), ...
|
||||
'DisplayName', 'IFF + HPF')
|
||||
plot(freqs, abs(squeeze(freqresp(G_cl_iff_kp('Dx', 'Fdx'), freqs, 'rad/s'))), '-', 'color', colors(2,:), ...
|
||||
'DisplayName', 'IFF + $k_p$')
|
||||
plot(freqs, abs(squeeze(freqresp(G_cl_rdc( 'Dx', 'Fdx'), freqs, 'rad/s'))), '-', 'color', colors(3,:), ...
|
||||
'DisplayName', 'RDC')
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [rad/s]'); ylabel('Compliance [m/N]');
|
||||
ldg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
|
||||
ldg.ItemTokenSize = [10, 1];
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
682
A2-nass-rotating-3dof-model/rotating_7_nano_hexapod.m
Normal file
682
A2-nass-rotating-3dof-model/rotating_7_nano_hexapod.m
Normal file
@ -0,0 +1,682 @@
|
||||
%% Clear Workspace and Close figures
|
||||
clear; close all; clc;
|
||||
|
||||
%% Intialize Laplace variable
|
||||
s = zpk('s');
|
||||
|
||||
%% Path for functions, data and scripts
|
||||
addpath('./mat/'); % Path for data
|
||||
addpath('./src/'); % Path for Functions
|
||||
|
||||
%% Colors for the figures
|
||||
colors = colororder;
|
||||
|
||||
%% Simscape model name
|
||||
mdl = 'rotating_model';
|
||||
|
||||
%% Nano-Hexapod
|
||||
mn = 15; % Nano-Hexapod mass [kg]
|
||||
|
||||
%% Light Sample
|
||||
ms = 1; % Sample Mass [kg]
|
||||
|
||||
%% General Configuration
|
||||
model_config = struct();
|
||||
model_config.controller = "open_loop"; % Default: Open-Loop
|
||||
model_config.Tuv_type = "normal"; % Default: 2DoF stage
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/controller'], 1, 'openinput'); io_i = io_i + 1; % [Fu, Fv]
|
||||
io(io_i) = linio([mdl, '/fd'], 1, 'openinput'); io_i = io_i + 1; % [Fdx, Fdy]
|
||||
io(io_i) = linio([mdl, '/xf'], 1, 'openinput'); io_i = io_i + 1; % [Dfx, Dfy]
|
||||
io(io_i) = linio([mdl, '/translation_stage'], 1, 'openoutput'); io_i = io_i + 1; % [Fmu, Fmv]
|
||||
io(io_i) = linio([mdl, '/translation_stage'], 2, 'openoutput'); io_i = io_i + 1; % [Du, Dv]
|
||||
io(io_i) = linio([mdl, '/ext_metrology'], 1, 'openoutput'); io_i = io_i + 1; % [Dx, Dy]
|
||||
|
||||
%% Voice Coil (i.e. soft) Nano-Hexapod
|
||||
kn = 1e4; % Nano-Hexapod Stiffness [N/m]
|
||||
cn = 2*0.005*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
|
||||
|
||||
Wz = 0; % Rotating Velocity [rad/s]
|
||||
G_vc_norot = linearize(mdl, io, 0.0);
|
||||
G_vc_norot.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
|
||||
G_vc_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
Wz = 2*pi; % Rotating Velocity [rad/s]
|
||||
G_vc_fast = linearize(mdl, io, 0.0);
|
||||
G_vc_fast.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
|
||||
G_vc_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
%% APA (i.e. relatively stiff) Nano-Hexapod
|
||||
kn = 1e6; % Nano-Hexapod Stiffness [N/m]
|
||||
cn = 2*0.005*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
|
||||
|
||||
Wz = 0; % Rotating Velocity [rad/s]
|
||||
G_md_norot = linearize(mdl, io, 0.0);
|
||||
G_md_norot.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
|
||||
G_md_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
Wz = 2*pi; % Rotating Velocity [rad/s]
|
||||
G_md_fast = linearize(mdl, io, 0.0);
|
||||
G_md_fast.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
|
||||
G_md_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
%% Piezoelectric (i.e. stiff) Nano-Hexapod
|
||||
kn = 1e8; % Nano-Hexapod Stiffness [N/m]
|
||||
cn = 2*0.005*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
|
||||
|
||||
Wz = 0; % Rotating Velocity [rad/s]
|
||||
G_pz_norot = linearize(mdl, io, 0.0);
|
||||
G_pz_norot.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
|
||||
G_pz_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
Wz = 2*pi; % Rotating Velocity [rad/s]
|
||||
G_pz_fast = linearize(mdl, io, 0.0);
|
||||
G_pz_fast.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
|
||||
G_pz_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
%% Compute negative spring in [N/m]
|
||||
Kneg_light = (15+1)*(2*pi)^2;
|
||||
|
||||
%% Effect of rotation on the nano-hexapod dynamics
|
||||
freqs = logspace(0, 1, 1000);
|
||||
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_norot('Du', 'Fu'), freqs, 'Hz'))), '--', 'color', colors(1,:), ...
|
||||
'DisplayName', '$\Omega = 0\,$rpm, $D_u/F_u$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_fast( 'Du', 'Fu'), freqs, 'Hz'))), '-' , 'color', colors(1,:), ...
|
||||
'DisplayName', '$\Omega = 60\,$rpm, $D_u/F_u$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_fast( 'Dv', 'Fu'), freqs, 'Hz'))), '-' , 'color', [colors(1,:), 0.5], ...
|
||||
'DisplayName', '$\Omega = 60\,$rpm, $D_v/F_u$');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
ylim([1e-6, 1e-2])
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_vc_norot('Du', 'Fu'), freqs, 'Hz'))), '--', 'color', colors(1,:));
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_vc_fast( 'Du', 'Fu'), freqs, 'Hz'))), '-' , 'color', colors(1,:));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||||
hold off;
|
||||
yticks(-360:90:360);
|
||||
ylim([-180, 0]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
% xlim([1, 1e3]);
|
||||
|
||||
%% Effect of rotation on the nano-hexapod dynamics
|
||||
freqs = logspace(1, 2, 1000);
|
||||
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_norot('Du', 'Fu'), freqs, 'Hz'))), '--', 'color', colors(2,:), ...
|
||||
'DisplayName', '$\Omega = 0\,$rpm, $D_u/F_u$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_fast( 'Du', 'Fu'), freqs, 'Hz'))), '-' , 'color', colors(2,:), ...
|
||||
'DisplayName', '$\Omega = 60\,$rpm, $D_u/F_u$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_fast( 'Dv', 'Fu'), freqs, 'Hz'))), '-' , 'color', [colors(2,:), 0.5], ...
|
||||
'DisplayName', '$\Omega = 60\,$rpm, $D_v/F_u$');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
ylim([1e-8, 1e-4])
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_md_norot('Du', 'Fu'), freqs, 'Hz'))), '--', 'color', colors(2,:));
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_md_fast( 'Du', 'Fu'), freqs, 'Hz'))), '-' , 'color', colors(2,:));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||||
hold off;
|
||||
yticks(-360:90:360);
|
||||
ylim([-180, 0]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
|
||||
%% Effect of rotation on the nano-hexapod dynamics
|
||||
freqs = logspace(2, 3, 1000);
|
||||
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_norot('Du', 'Fu'), freqs, 'Hz'))), '--', 'color', colors(3,:), ...
|
||||
'DisplayName', '$\Omega = 0\,$rpm, $D_u/F_u$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_fast( 'Du', 'Fu'), freqs, 'Hz'))), '-' , 'color', colors(3,:), ...
|
||||
'DisplayName', '$\Omega = 60\,$rpm, $D_u/F_u$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_fast( 'Dv', 'Fu'), freqs, 'Hz'))), '-' , 'color', [colors(3,:), 0.5], ...
|
||||
'DisplayName', '$\Omega = 60\,$rpm, $D_v/F_u$');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
ylim([1e-10, 1e-6])
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_pz_norot('Du', 'Fu'), freqs, 'Hz'))), '--', 'color', colors(3,:));
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_pz_fast( 'Du', 'Fu'), freqs, 'Hz'))), '-' , 'color', colors(3,:));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||||
hold off;
|
||||
yticks(-360:90:360);
|
||||
ylim([-180, 0]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
|
||||
%% Compute the optimal control gain
|
||||
wis = logspace(-2, 3, 200); % [rad/s]
|
||||
|
||||
opt_iff_hpf_xi_vc = zeros(1, length(wis)); % Optimal simultaneous damping
|
||||
opt_iff_hpf_gain_vc = zeros(1, length(wis)); % Corresponding optimal gain
|
||||
|
||||
opt_iff_hpf_xi_md = zeros(1, length(wis)); % Optimal simultaneous damping
|
||||
opt_iff_hpf_gain_md = zeros(1, length(wis)); % Corresponding optimal gain
|
||||
|
||||
opt_iff_hpf_xi_pz = zeros(1, length(wis)); % Optimal simultaneous damping
|
||||
opt_iff_hpf_gain_pz = zeros(1, length(wis)); % Corresponding optimal gain
|
||||
|
||||
for wi_i = 1:length(wis)
|
||||
wi = wis(wi_i);
|
||||
Kiff = 1/(s + wi)*eye(2);
|
||||
|
||||
fun = @(g)computeSimultaneousDamping(g, G_vc_fast({'fu', 'fv'}, {'Fu', 'Fv'}), Kiff);
|
||||
|
||||
[g_opt, xi_opt] = fminsearch(fun, 0.1);
|
||||
opt_iff_hpf_xi_vc(wi_i) = 1/xi_opt;
|
||||
opt_iff_hpf_gain_vc(wi_i) = g_opt;
|
||||
|
||||
fun = @(g)computeSimultaneousDamping(g, G_md_fast({'fu', 'fv'}, {'Fu', 'Fv'}), Kiff);
|
||||
|
||||
[g_opt, xi_opt] = fminsearch(fun, 0.1);
|
||||
opt_iff_hpf_xi_md(wi_i) = 1/xi_opt;
|
||||
opt_iff_hpf_gain_md(wi_i) = g_opt;
|
||||
|
||||
fun = @(g)computeSimultaneousDamping(g, G_pz_fast({'fu', 'fv'}, {'Fu', 'Fv'}), Kiff);
|
||||
|
||||
[g_opt, xi_opt] = fminsearch(fun, 0.1);
|
||||
opt_iff_hpf_xi_pz(wi_i) = 1/xi_opt;
|
||||
opt_iff_hpf_gain_pz(wi_i) = g_opt;
|
||||
end
|
||||
|
||||
%% Find optimal parameters with at least a gain margin of 2
|
||||
i_iff_hpf_vc = find(opt_iff_hpf_gain_vc < 0.5*(wis*((sqrt(1e4/16)/(2*pi))^2 - 1)));
|
||||
i_iff_hpf_vc = i_iff_hpf_vc(1);
|
||||
|
||||
i_iff_hpf_md = find(opt_iff_hpf_xi_md > 0.95*max(opt_iff_hpf_xi_md));
|
||||
i_iff_hpf_md = i_iff_hpf_md(end)+1;
|
||||
|
||||
i_iff_hpf_pz = find(opt_iff_hpf_xi_pz > 0.95*max(opt_iff_hpf_xi_pz));
|
||||
i_iff_hpf_pz = i_iff_hpf_pz(end)+1;
|
||||
|
||||
%% Optimal modified IFF parameters that yields maximum simultaneous damping
|
||||
figure;
|
||||
yyaxis left
|
||||
hold on;
|
||||
plot(wis, opt_iff_hpf_xi_vc, '-', 'DisplayName', '$\xi_{cl}$');
|
||||
plot(wis(i_iff_hpf_vc), opt_iff_hpf_xi_vc(i_iff_hpf_vc), '.', 'MarkerSize', 15, 'HandleVisibility', 'off');
|
||||
hold off;
|
||||
set(gca, 'YScale', 'lin');
|
||||
ylim([0,1]);
|
||||
ylabel('Damping Ratio $\xi$');
|
||||
|
||||
yyaxis right
|
||||
hold on;
|
||||
plot(wis, opt_iff_hpf_gain_vc, '-', 'DisplayName', '$g_{opt}$');
|
||||
plot(wis, wis*((sqrt(1e4/16)/(2*pi))^2 - 1), '--', 'DisplayName', '$g_{max}$');
|
||||
plot(wis(i_iff_hpf_vc), opt_iff_hpf_gain_vc(i_iff_hpf_vc), '.', 'MarkerSize', 15, 'HandleVisibility', 'off');
|
||||
set(gca, 'YScale', 'lin');
|
||||
ylim([0,200]);
|
||||
xlabel('$\omega_i$ [rad/s]');
|
||||
set(gca, 'YTickLabel',[]);
|
||||
ylabel('Controller gain $g$');
|
||||
set(gca, 'XScale', 'log');
|
||||
xticks([1e-2,1,1e2])
|
||||
legend('location', 'northwest', 'FontSize', 8);
|
||||
|
||||
figure;
|
||||
yyaxis left
|
||||
hold on;
|
||||
plot(wis, opt_iff_hpf_xi_md, '-');
|
||||
plot(wis(i_iff_hpf_md), opt_iff_hpf_xi_md(i_iff_hpf_md), '.', 'MarkerSize', 15, 'HandleVisibility', 'off');
|
||||
hold off;
|
||||
set(gca, 'YScale', 'lin');
|
||||
ylim([0,1]);
|
||||
ylabel('Damping Ratio $\xi$');
|
||||
|
||||
yyaxis right
|
||||
hold on;
|
||||
plot(wis, opt_iff_hpf_gain_md, '-');
|
||||
plot(wis(i_iff_hpf_md), opt_iff_hpf_gain_md(i_iff_hpf_md), '.', 'MarkerSize', 15, 'HandleVisibility', 'off');
|
||||
plot(wis, wis*((sqrt(1e6/16)/(2*pi))^2 - 1), '--');
|
||||
set(gca, 'YScale', 'lin');
|
||||
ylim([0,1000]);
|
||||
xlabel('$\omega_i$ [rad/s]');
|
||||
ylabel('Controller gain $g$');
|
||||
set(gca, 'YTickLabel',[]);
|
||||
set(gca, 'XScale', 'log');
|
||||
xticks([1e-2,1,1e2])
|
||||
|
||||
figure;
|
||||
yyaxis left
|
||||
hold on;
|
||||
plot(wis, opt_iff_hpf_xi_pz, '-');
|
||||
plot(wis(i_iff_hpf_pz), opt_iff_hpf_xi_pz(i_iff_hpf_pz), '.', 'MarkerSize', 15, 'HandleVisibility', 'off');
|
||||
hold off;
|
||||
set(gca, 'YScale', 'lin');
|
||||
ylim([0,1]);
|
||||
ylabel('Damping Ratio $\xi$');
|
||||
|
||||
yyaxis right
|
||||
hold on;
|
||||
plot(wis, opt_iff_hpf_gain_pz, '-');
|
||||
plot(wis(i_iff_hpf_pz), opt_iff_hpf_gain_pz(i_iff_hpf_pz), '.', 'MarkerSize', 15, 'HandleVisibility', 'off');
|
||||
plot(wis, wis*((sqrt(1e8/16)/(2*pi))^2 - 1), '--');
|
||||
set(gca, 'YScale', 'lin');
|
||||
ylim([0,10000]);
|
||||
xlabel('$\omega_i$ [rad/s]');
|
||||
set(gca, 'YTickLabel',[]);
|
||||
ylabel('Controller gain $g$');
|
||||
set(gca, 'XScale', 'log');
|
||||
xticks([1e-2,1,1e2])
|
||||
|
||||
%% Maximum rotating velocity
|
||||
Wz = 2*pi; % [rad/s]
|
||||
|
||||
%% Minimum parallel stiffness
|
||||
kp_min = (mn + ms) * Wz^2; % [N/m]
|
||||
|
||||
%% Parameters for simulation
|
||||
mn = 15; % Nano-Hexapod mass [kg]
|
||||
ms = 1; % Sample Mass [kg]
|
||||
|
||||
%% IFF Controller
|
||||
Kiff_vc = 1/(s + 0.1*sqrt(1e4/(mn+ms)))*eye(2); % IFF
|
||||
Kiff_md = 1/(s + 0.1*sqrt(1e6/(mn+ms)))*eye(2); % IFF
|
||||
Kiff_pz = 1/(s + 0.1*sqrt(1e8/(mn+ms)))*eye(2); % IFF
|
||||
|
||||
%% General Configuration
|
||||
model_config = struct();
|
||||
model_config.controller = "open_loop"; % Default: Open-Loop
|
||||
model_config.Tuv_type = "parallel_k"; % Default: 2DoF stage
|
||||
|
||||
%% Computes the optimal parameters and attainable simultaneous damping - Voice Coil nano-hexapod
|
||||
kps_vc = logspace(log10(kp_min), log10(1e4), 100); % Tested parallel stiffnesses [N/m]
|
||||
kps_vc(end) = [];
|
||||
|
||||
opt_iff_kp_xi_vc = zeros(1, length(kps_vc)); % Optimal simultaneous damping
|
||||
opt_iff_kp_gain_vc = zeros(1, length(kps_vc)); % Corresponding optimal gain
|
||||
|
||||
for kp_i = 1:length(kps_vc)
|
||||
% Voice Coil Nano-Hexapod
|
||||
kp = kps_vc(kp_i);
|
||||
cp = 2*0.001*sqrt((ms + mn)*kp);
|
||||
kn = 1e4 - kp; % Nano-Hexapod Stiffness [N/m]
|
||||
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
|
||||
|
||||
% Identify dynamics
|
||||
Giff_vc = linearize(mdl, io, 0);
|
||||
Giff_vc.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
|
||||
Giff_vc.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
fun = @(g)computeSimultaneousDamping(g, Giff_vc({'fu', 'fv'}, {'Fu', 'Fv'}), Kiff_vc);
|
||||
|
||||
[g_opt, xi_opt] = fminsearch(fun, 0.1);
|
||||
opt_iff_kp_xi_vc(kp_i) = 1/xi_opt;
|
||||
opt_iff_kp_gain_vc(kp_i) = g_opt;
|
||||
end
|
||||
|
||||
%% Computes the optimal parameters and attainable simultaneous damping - APA nano-hexapod
|
||||
kps_md = logspace(log10(kp_min), log10(1e6), 100); % Tested parallel stiffnesses [N/m]
|
||||
kps_md(end) = [];
|
||||
|
||||
opt_iff_kp_xi_md = zeros(1, length(kps_md)); % Optimal simultaneous damping
|
||||
opt_iff_kp_gain_md = zeros(1, length(kps_md)); % Corresponding optimal gain
|
||||
|
||||
|
||||
for kp_i = 1:length(kps_md)
|
||||
% Voice Coil Nano-Hexapod
|
||||
kp = kps_md(kp_i);
|
||||
cp = 2*0.001*sqrt((ms + mn)*kp);
|
||||
kn = 1e6 - kp; % Nano-Hexapod Stiffness [N/m]
|
||||
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
|
||||
|
||||
% Identify dynamics
|
||||
Giff_md = linearize(mdl, io, 0);
|
||||
Giff_md.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
|
||||
Giff_md.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
fun = @(g)computeSimultaneousDamping(g, Giff_md({'fu', 'fv'}, {'Fu', 'Fv'}), Kiff_md);
|
||||
|
||||
[g_opt, xi_opt] = fminsearch(fun, 0.1);
|
||||
opt_iff_kp_xi_md(kp_i) = 1/xi_opt;
|
||||
opt_iff_kp_gain_md(kp_i) = g_opt;
|
||||
end
|
||||
|
||||
%% Computes the optimal parameters and attainable simultaneous damping - Piezo nano-hexapod
|
||||
kps_pz = logspace(log10(kp_min), log10(1e8), 100); % Tested parallel stiffnesses [N/m]
|
||||
kps_pz(end) = [];
|
||||
|
||||
opt_iff_kp_xi_pz = zeros(1, length(kps_pz)); % Optimal simultaneous damping
|
||||
opt_iff_kp_gain_pz = zeros(1, length(kps_pz)); % Corresponding optimal gain
|
||||
|
||||
|
||||
for kp_i = 1:length(kps_pz)
|
||||
% Voice Coil Nano-Hexapod
|
||||
kp = kps_pz(kp_i);
|
||||
cp = 2*0.001*sqrt((ms + mn)*kp);
|
||||
kn = 1e8 - kp; % Nano-Hexapod Stiffness [N/m]
|
||||
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
|
||||
|
||||
% Identify dynamics
|
||||
Giff_pz = linearize(mdl, io, 0);
|
||||
Giff_pz.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
|
||||
Giff_pz.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
fun = @(g)computeSimultaneousDamping(g, Giff_pz({'fu', 'fv'}, {'Fu', 'Fv'}), Kiff_pz);
|
||||
|
||||
[g_opt, xi_opt] = fminsearch(fun, 0.1);
|
||||
opt_iff_kp_xi_pz(kp_i) = 1/xi_opt;
|
||||
opt_iff_kp_gain_pz(kp_i) = g_opt;
|
||||
end
|
||||
|
||||
%% Find result with wanted parallel stiffness
|
||||
[~, i_kp_vc] = min(abs(kps_vc - 1e3));
|
||||
[~, i_kp_md] = min(abs(kps_md - 1e4));
|
||||
[~, i_kp_pz] = min(abs(kps_pz - 1e6));
|
||||
|
||||
%% Identify plants with choosen Parallel stiffnesses
|
||||
model_config.Tuv_type = "parallel_k"; % Default: 2DoF stage
|
||||
|
||||
% Voice Coil
|
||||
kp = 1e3;
|
||||
cp = 2*0.001*sqrt((ms + mn)*kp);
|
||||
kn = 1e4-kp; % Nano-Hexapod Stiffness [N/m]
|
||||
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
|
||||
|
||||
% Identify dynamics
|
||||
Wz = 2*pi; % [rad/s]
|
||||
G_vc_kp_fast = linearize(mdl, io, 0);
|
||||
G_vc_kp_fast.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
|
||||
G_vc_kp_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
Wz = 0; % [rad/s]
|
||||
G_vc_kp_norot = linearize(mdl, io, 0);
|
||||
G_vc_kp_norot.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
|
||||
G_vc_kp_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
% APA
|
||||
kp = 1e4;
|
||||
cp = 2*0.001*sqrt((ms + mn)*kp);
|
||||
kn = 1e6 - kp; % Nano-Hexapod Stiffness [N/m]
|
||||
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
|
||||
|
||||
% Identify dynamics
|
||||
Wz = 2*pi; % [rad/s]
|
||||
G_md_kp_fast = linearize(mdl, io, 0);
|
||||
G_md_kp_fast.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
|
||||
G_md_kp_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
Wz = 0; % [rad/s]
|
||||
G_md_kp_norot = linearize(mdl, io, 0);
|
||||
G_md_kp_norot.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
|
||||
G_md_kp_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
% Piezo
|
||||
kp = 1e6;
|
||||
cp = 2*0.001*sqrt((ms + mn)*kp);
|
||||
kn = 1e8 - kp; % Nano-Hexapod Stiffness [N/m]
|
||||
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
|
||||
|
||||
% Identify dynamics
|
||||
Wz = 2*pi; % [rad/s]
|
||||
G_pz_kp_fast = linearize(mdl, io, 0);
|
||||
G_pz_kp_fast.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
|
||||
G_pz_kp_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
Wz = 0; % [rad/s]
|
||||
G_pz_kp_norot = linearize(mdl, io, 0);
|
||||
G_pz_kp_norot.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
|
||||
G_pz_kp_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
%% Optimal IFF gain and associated simultaneous damping as a function of the parallel stiffness
|
||||
figure;
|
||||
hold on;
|
||||
plot(kps_vc, opt_iff_kp_xi_vc, '-', ...
|
||||
'color', colors(1,:), 'DisplayName', '$k_n = 0.01\,N/\mu m$');
|
||||
plot(kps_vc(i_kp_vc), opt_iff_kp_xi_vc(i_kp_vc), '.', ...
|
||||
'color', colors(1,:), 'MarkerSize', 15, 'HandleVisibility', 'off');
|
||||
plot(kps_md, opt_iff_kp_xi_md, '-', ...
|
||||
'color', colors(2,:), 'DisplayName', '$k_n = 1\,N/\mu m$');
|
||||
plot(kps_md(i_kp_md), opt_iff_kp_xi_md(i_kp_md), '.', ...
|
||||
'color', colors(2,:), 'MarkerSize', 15, 'HandleVisibility', 'off');
|
||||
plot(kps_pz, opt_iff_kp_xi_pz, '-', ...
|
||||
'color', colors(3,:), 'DisplayName', '$k_n = 100\,N/\mu m$');
|
||||
plot(kps_pz(i_kp_pz), opt_iff_kp_xi_pz(i_kp_pz), '.', ...
|
||||
'color', colors(3,:), 'MarkerSize', 15, 'HandleVisibility', 'off');
|
||||
hold off;
|
||||
xlabel('$k_p [N/m]$');
|
||||
ylabel('Damping Ratio $\xi$');
|
||||
set(gca, 'XScale', 'log');
|
||||
set(gca, 'YScale', 'lin');
|
||||
ylim([0,1]);
|
||||
yticks([0:0.2:1])
|
||||
legend('location', 'southeast', 'FontSize', 8);
|
||||
xlim([kps_pz(1), kps_pz(end)])
|
||||
|
||||
%% Computes the optimal parameters and attainable simultaneous damping - Piezo nano-hexapod
|
||||
rdc_gains = 2*logspace(1, 5, 200);
|
||||
% Obtained simultaneous damping
|
||||
rdc_xi_vc = zeros(1, length(rdc_gains));
|
||||
rdc_xi_md = zeros(1, length(rdc_gains));
|
||||
rdc_xi_pz = zeros(1, length(rdc_gains));
|
||||
|
||||
Krdc = s*eye(2);
|
||||
Krdc.InputName = {'Du', 'Dv'};
|
||||
Krdc.OutputName = {'Fu', 'Fv'};
|
||||
|
||||
for g_i = 1:length(rdc_gains)
|
||||
[~, xi] = damp(feedback(G_vc_fast({'Du', 'Dv'}, {'Fu', 'Fv'}), rdc_gains(g_i)*Krdc));
|
||||
rdc_xi_vc(g_i) = min(xi);
|
||||
|
||||
[~, xi] = damp(feedback(G_md_fast({'Du', 'Dv'}, {'Fu', 'Fv'}), rdc_gains(g_i)*Krdc));
|
||||
rdc_xi_md(g_i) = min(xi);
|
||||
|
||||
[~, xi] = damp(feedback(G_pz_fast({'Du', 'Dv'}, {'Fu', 'Fv'}), rdc_gains(g_i)*Krdc));
|
||||
rdc_xi_pz(g_i) = min(xi);
|
||||
end
|
||||
|
||||
%% Optimal RDC
|
||||
[~, i_rdc_vc] = min(abs(rdc_xi_vc - 0.99));
|
||||
[~, i_rdc_md] = min(abs(rdc_xi_md - 0.99));
|
||||
[~, i_rdc_pz] = min(abs(rdc_xi_pz - 0.99));
|
||||
|
||||
Krdc_vc = rdc_gains(i_rdc_vc)*Krdc;
|
||||
Krdc_md = rdc_gains(i_rdc_md)*Krdc;
|
||||
Krdc_pz = rdc_gains(i_rdc_pz)*Krdc;
|
||||
|
||||
%% Optimal IFF gain and associated simultaneous damping as a function of the parallel stiffness
|
||||
figure;
|
||||
hold on;
|
||||
plot(rdc_gains, rdc_xi_vc, '-', ...
|
||||
'color', colors(1,:), 'DisplayName', '$k_n = 0.01\,N/\mu m$');
|
||||
plot(rdc_gains(i_rdc_vc), rdc_xi_vc(i_rdc_vc), '.', ...
|
||||
'color', colors(1,:), 'MarkerSize', 15, 'HandleVisibility', 'off');
|
||||
plot(rdc_gains, rdc_xi_md, '-', ...
|
||||
'color', colors(2,:), 'DisplayName', '$k_n = 1\,N/\mu m$');
|
||||
plot(rdc_gains(i_rdc_md), rdc_xi_md(i_rdc_md), '.', ...
|
||||
'color', colors(2,:), 'MarkerSize', 15, 'HandleVisibility', 'off');
|
||||
plot(rdc_gains, rdc_xi_pz, '-', ...
|
||||
'color', colors(3,:), 'DisplayName', '$k_n = 100\,N/\mu m$');
|
||||
plot(rdc_gains(i_rdc_pz), rdc_xi_pz(i_rdc_pz), '.', ...
|
||||
'color', colors(3,:), 'MarkerSize', 15, 'HandleVisibility', 'off');
|
||||
hold off;
|
||||
xlabel('Relative Damping Controller gain $g$');
|
||||
ylabel('Damping Ratio $\xi$');
|
||||
set(gca, 'XScale', 'log');
|
||||
set(gca, 'YScale', 'lin');
|
||||
ylim([0,1]);
|
||||
yticks([0:0.2:1])
|
||||
xlim([rdc_gains(1), rdc_gains(end)])
|
||||
legend('location', 'southeast', 'FontSize', 8);
|
||||
|
||||
%% Closed-Loop Plants - IFF with HPF
|
||||
G_vc_norot_iff_hpf = feedback(G_vc_norot, Kiff_hpf_vc, 'name');
|
||||
G_vc_fast_iff_hpf = feedback(G_vc_fast, Kiff_hpf_vc, 'name');
|
||||
|
||||
G_md_norot_iff_hpf = feedback(G_md_norot, Kiff_hpf_md, 'name');
|
||||
G_md_fast_iff_hpf = feedback(G_md_fast, Kiff_hpf_md, 'name');
|
||||
|
||||
G_pz_norot_iff_hpf = feedback(G_pz_norot, Kiff_hpf_pz, 'name');
|
||||
G_pz_fast_iff_hpf = feedback(G_pz_fast, Kiff_hpf_pz, 'name');
|
||||
|
||||
%% Closed-Loop Plants - IFF with Parallel Stiffness
|
||||
G_vc_norot_iff_kp = feedback(G_vc_kp_norot, Kiff_kp_vc, 'name');
|
||||
G_vc_fast_iff_kp = feedback(G_vc_kp_fast, Kiff_kp_vc, 'name');
|
||||
|
||||
G_md_norot_iff_kp = feedback(G_md_kp_norot, Kiff_kp_md, 'name');
|
||||
G_md_fast_iff_kp = feedback(G_md_kp_fast, Kiff_kp_md, 'name');
|
||||
|
||||
G_pz_norot_iff_kp = feedback(G_pz_kp_norot, Kiff_kp_pz, 'name');
|
||||
G_pz_fast_iff_kp = feedback(G_pz_kp_fast, Kiff_kp_pz, 'name');
|
||||
|
||||
%% Closed-Loop Plants - RDC
|
||||
G_vc_norot_rdc = feedback(G_vc_norot, Krdc_vc, 'name');
|
||||
G_vc_fast_rdc = feedback(G_vc_fast, Krdc_vc, 'name');
|
||||
|
||||
G_md_norot_rdc = feedback(G_md_norot, Krdc_md, 'name');
|
||||
G_md_fast_rdc = feedback(G_md_fast, Krdc_md, 'name');
|
||||
|
||||
G_pz_norot_rdc = feedback(G_pz_norot, Krdc_pz, 'name');
|
||||
G_pz_fast_rdc = feedback(G_pz_fast, Krdc_pz, 'name');
|
||||
|
||||
%% Comparison of the damped plants (direct and coupling terms) for the three proposed active damping techniques (IFF with HPF, IFF with $k_p$ and RDC) applied on the three nano-hexapod stiffnesses
|
||||
freqs_vc = logspace(-1, 2, 1000);
|
||||
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast( 'Du', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [zeros(1,3)]);
|
||||
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast( 'Dv', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [zeros(1,3), 0.5]);
|
||||
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_iff_hpf( 'Du', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [colors(1,:)]);
|
||||
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_iff_hpf( 'Dv', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [colors(1,:), 0.5]);
|
||||
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_iff_kp( 'Du', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [colors(2,:)]);
|
||||
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_iff_kp( 'Dv', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [colors(2,:), 0.5]);
|
||||
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_rdc( 'Du', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [colors(3,:)]);
|
||||
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_rdc( 'Dv', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [colors(3,:), 0.5]);
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
ylim([1e-8, 1e-2])
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
plot(freqs_vc, 180/pi*angle(squeeze(freqresp(G_vc_fast( 'Du', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [zeros(1,3)]);
|
||||
plot(freqs_vc, 180/pi*angle(squeeze(freqresp(G_vc_fast_iff_hpf( 'Du', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [colors(1,:)]);
|
||||
plot(freqs_vc, 180/pi*angle(squeeze(freqresp(G_vc_fast_iff_kp( 'Du', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [colors(2,:)]);
|
||||
plot(freqs_vc, 180/pi*angle(squeeze(freqresp(G_vc_fast_rdc( 'Du', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [colors(3,:)]);
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||||
hold off;
|
||||
yticks(-360:90:360);
|
||||
ylim([-180, 0]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([freqs_vc(1), freqs_vc(end)]);
|
||||
|
||||
freqs_md = logspace(0, 3, 1000);
|
||||
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(freqs_md, abs(squeeze(freqresp(G_md_fast( 'Du', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [zeros(1,3)]);
|
||||
plot(freqs_md, abs(squeeze(freqresp(G_md_fast( 'Dv', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [zeros(1,3), 0.5]);
|
||||
plot(freqs_md, abs(squeeze(freqresp(G_md_fast_iff_hpf( 'Du', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [colors(1,:)]);
|
||||
plot(freqs_md, abs(squeeze(freqresp(G_md_fast_iff_hpf( 'Dv', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [colors(1,:), 0.5]);
|
||||
plot(freqs_md, abs(squeeze(freqresp(G_md_fast_iff_kp( 'Du', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [colors(2,:)]);
|
||||
plot(freqs_md, abs(squeeze(freqresp(G_md_fast_iff_kp( 'Dv', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [colors(2,:), 0.5]);
|
||||
plot(freqs_md, abs(squeeze(freqresp(G_md_fast_rdc( 'Du', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [colors(3,:)]);
|
||||
plot(freqs_md, abs(squeeze(freqresp(G_md_fast_rdc( 'Dv', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [colors(3,:), 0.5]);
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
ylim([1e-10, 1e-4])
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
plot(freqs_md, 180/pi*angle(squeeze(freqresp(G_md_fast( 'Du', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [zeros(1,3)]);
|
||||
plot(freqs_md, 180/pi*angle(squeeze(freqresp(G_md_fast_iff_hpf( 'Du', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [colors(1,:)]);
|
||||
plot(freqs_md, 180/pi*angle(squeeze(freqresp(G_md_fast_iff_kp( 'Du', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [colors(2,:)]);
|
||||
plot(freqs_md, 180/pi*angle(squeeze(freqresp(G_md_fast_rdc( 'Du', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [colors(3,:)]);
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||||
hold off;
|
||||
yticks(-360:90:360);
|
||||
ylim([-180, 0]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([freqs_md(1), freqs_md(end)]);
|
||||
|
||||
freqs_pz = logspace(0, 3, 1000);
|
||||
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast( 'Du', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [zeros(1,3)], ...
|
||||
'DisplayName', 'OL');
|
||||
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast( 'Dv', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [zeros(1,3), 0.5], ...
|
||||
'DisplayName', 'Coupling');
|
||||
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_iff_hpf( 'Du', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [colors(1,:)], ...
|
||||
'DisplayName', 'IFF + HPF');
|
||||
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_iff_hpf( 'Dv', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [colors(1,:), 0.5], ...
|
||||
'HandleVisibility', 'off');
|
||||
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_iff_kp( 'Du', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [colors(2,:)], ...
|
||||
'DisplayName', 'IFF + $k_p$');
|
||||
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_iff_kp( 'Dv', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [colors(2,:), 0.5], ...
|
||||
'HandleVisibility', 'off');
|
||||
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_rdc( 'Du', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [colors(3,:)], ...
|
||||
'DisplayName', 'RDC');
|
||||
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_rdc( 'Dv', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [colors(3,:), 0.5], ...
|
||||
'HandleVisibility', 'off');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
ylim([1e-12, 1e-6])
|
||||
ldg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
||||
ldg.ItemTokenSize = [20, 1];
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
plot(freqs_pz, 180/pi*angle(squeeze(freqresp(G_pz_fast( 'Du', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [zeros(1,3)]);
|
||||
plot(freqs_pz, 180/pi*angle(squeeze(freqresp(G_pz_fast_iff_hpf( 'Du', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [colors(1,:)]);
|
||||
plot(freqs_pz, 180/pi*angle(squeeze(freqresp(G_pz_fast_iff_kp( 'Du', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [colors(2,:)]);
|
||||
plot(freqs_pz, 180/pi*angle(squeeze(freqresp(G_pz_fast_rdc( 'Du', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [colors(3,:)]);
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||||
hold off;
|
||||
yticks(-360:90:360);
|
||||
ylim([-180, 0]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([freqs_pz(1), freqs_pz(end)]);
|
||||
470
A2-nass-rotating-3dof-model/rotating_8_nass.m
Normal file
470
A2-nass-rotating-3dof-model/rotating_8_nass.m
Normal file
@ -0,0 +1,470 @@
|
||||
%% Clear Workspace and Close figures
|
||||
clear; close all; clc;
|
||||
|
||||
%% Intialize Laplace variable
|
||||
s = zpk('s');
|
||||
|
||||
%% Path for functions, data and scripts
|
||||
addpath('./mat/'); % Path for data
|
||||
addpath('./src/'); % Path for Functions
|
||||
|
||||
%% Colors for the figures
|
||||
colors = colororder;
|
||||
|
||||
%% Nano-Hexapod on top of Micro-Station model
|
||||
mdl = 'nass_rotating_model';
|
||||
|
||||
%% Load micro-station parameters
|
||||
load('uniaxial_micro_station_parameters.mat')
|
||||
|
||||
%% Load controllers
|
||||
load('nass_controllers.mat');
|
||||
|
||||
%% System parameters
|
||||
mn = 15; % Nano-Hexapod mass [kg]
|
||||
ms = 1; % Sample Mass [kg]
|
||||
|
||||
% General Configuration
|
||||
model_config = struct();
|
||||
model_config.controller = "open_loop"; % Default: Open-Loop
|
||||
model_config.Tuv_type = "normal"; % Default: 2DoF stage
|
||||
|
||||
% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Forces [Fu, Fv]
|
||||
io(io_i) = linio([mdl, '/fd'], 1, 'openinput'); io_i = io_i + 1; % Direct Forces on Sample [Fdx, Fdy]
|
||||
io(io_i) = linio([mdl, '/xf'], 1, 'openinput'); io_i = io_i + 1; % Floor Motion [Dfx, Dfy]
|
||||
io(io_i) = linio([mdl, '/ft'], 1, 'openinput'); io_i = io_i + 1; % Micro-Station Disturbances [Ftx, Fty]
|
||||
io(io_i) = linio([mdl, '/nano_hexapod'], 1, 'openoutput'); io_i = io_i + 1; % [Fmu, Fmv]
|
||||
io(io_i) = linio([mdl, '/nano_hexapod'], 2, 'openoutput'); io_i = io_i + 1; % [Du, Dv]
|
||||
io(io_i) = linio([mdl, '/ext_metrology'],1, 'openoutput'); io_i = io_i + 1; % [Dx, Dy]
|
||||
|
||||
%% Identify plant without parallel stiffness
|
||||
% Voice Coil (i.e. soft) Nano-Hexapod
|
||||
kn = 1e4; % Nano-Hexapod Stiffness [N/m]
|
||||
cn = 2*0.005*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
|
||||
|
||||
Wz = 0; % Rotating Velocity [rad/s]
|
||||
G_vc_norot = linearize(mdl, io, 0.0);
|
||||
G_vc_norot.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
|
||||
G_vc_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
Wz = 2*pi; % Rotating Velocity [rad/s]
|
||||
G_vc_fast = linearize(mdl, io, 0.0);
|
||||
G_vc_fast.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
|
||||
G_vc_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
% APA (i.e. relatively stiff) Nano-Hexapod
|
||||
kn = 1e6; % Nano-Hexapod Stiffness [N/m]
|
||||
cn = 2*0.005*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
|
||||
|
||||
Wz = 0; % Rotating Velocity [rad/s]
|
||||
G_md_norot = linearize(mdl, io, 0.0);
|
||||
G_md_norot.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
|
||||
G_md_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
Wz = 2*pi; % Rotating Velocity [rad/s]
|
||||
G_md_fast = linearize(mdl, io, 0.0);
|
||||
G_md_fast.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
|
||||
G_md_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
% Piezoelectric (i.e. stiff) Nano-Hexapod
|
||||
kn = 1e8; % Nano-Hexapod Stiffness [N/m]
|
||||
cn = 2*0.005*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
|
||||
|
||||
Wz = 0; % Rotating Velocity [rad/s]
|
||||
G_pz_norot = linearize(mdl, io, 0.0);
|
||||
G_pz_norot.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
|
||||
G_pz_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
Wz = 2*pi; % Rotating Velocity [rad/s]
|
||||
G_pz_fast = linearize(mdl, io, 0.0);
|
||||
G_pz_fast.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
|
||||
G_pz_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
%% Identify plants with Parallel stiffnesses
|
||||
model_config.Tuv_type = "parallel_k"; % Default: 2DoF stage
|
||||
|
||||
% Voice Coil
|
||||
kp = 1e3;
|
||||
cp = 2*0.001*sqrt((ms + mn)*kp);
|
||||
kn = 1e4-kp; % Nano-Hexapod Stiffness [N/m]
|
||||
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
|
||||
|
||||
% Identify dynamics
|
||||
Wz = 2*pi; % [rad/s]
|
||||
G_vc_kp_fast = linearize(mdl, io, 0);
|
||||
G_vc_kp_fast.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
|
||||
G_vc_kp_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
Wz = 0; % [rad/s]
|
||||
G_vc_kp_norot = linearize(mdl, io, 0);
|
||||
G_vc_kp_norot.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
|
||||
G_vc_kp_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
% APA
|
||||
kp = 1e4;
|
||||
cp = 2*0.001*sqrt((ms + mn)*kp);
|
||||
kn = 1e6 - kp; % Nano-Hexapod Stiffness [N/m]
|
||||
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
|
||||
|
||||
% Identify dynamics
|
||||
Wz = 2*pi; % [rad/s]
|
||||
G_md_kp_fast = linearize(mdl, io, 0);
|
||||
G_md_kp_fast.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
|
||||
G_md_kp_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
Wz = 0; % [rad/s]
|
||||
G_md_kp_norot = linearize(mdl, io, 0);
|
||||
G_md_kp_norot.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
|
||||
G_md_kp_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
% Piezo
|
||||
kp = 1e5;
|
||||
cp = 2*0.001*sqrt((ms + mn)*kp);
|
||||
kn = 1e8 - kp; % Nano-Hexapod Stiffness [N/m]
|
||||
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
|
||||
|
||||
% Identify dynamics
|
||||
Wz = 2*pi; % [rad/s]
|
||||
G_pz_kp_fast = linearize(mdl, io, 0);
|
||||
G_pz_kp_fast.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
|
||||
G_pz_kp_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
Wz = 0; % [rad/s]
|
||||
G_pz_kp_norot = linearize(mdl, io, 0);
|
||||
G_pz_kp_norot.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
|
||||
G_pz_kp_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
%% Compute dampepd plants
|
||||
% Closed-Loop Plants - IFF with HPF
|
||||
G_vc_norot_iff_hpf = feedback(G_vc_norot, Kiff_hpf_vc, 'name');
|
||||
G_vc_fast_iff_hpf = feedback(G_vc_fast, Kiff_hpf_vc, 'name');
|
||||
|
||||
G_md_norot_iff_hpf = feedback(G_md_norot, Kiff_hpf_md, 'name');
|
||||
G_md_fast_iff_hpf = feedback(G_md_fast, Kiff_hpf_md, 'name');
|
||||
|
||||
G_pz_norot_iff_hpf = feedback(G_pz_norot, Kiff_hpf_pz, 'name');
|
||||
G_pz_fast_iff_hpf = feedback(G_pz_fast, Kiff_hpf_pz, 'name');
|
||||
|
||||
% Closed-Loop Plants - IFF with Parallel Stiffness
|
||||
G_vc_norot_iff_kp = feedback(G_vc_kp_norot, Kiff_kp_vc, 'name');
|
||||
G_vc_fast_iff_kp = feedback(G_vc_kp_fast, Kiff_kp_vc, 'name');
|
||||
|
||||
G_md_norot_iff_kp = feedback(G_md_kp_norot, Kiff_kp_md, 'name');
|
||||
G_md_fast_iff_kp = feedback(G_md_kp_fast, Kiff_kp_md, 'name');
|
||||
|
||||
G_pz_norot_iff_kp = feedback(G_pz_kp_norot, Kiff_kp_pz, 'name');
|
||||
G_pz_fast_iff_kp = feedback(G_pz_kp_fast, Kiff_kp_pz, 'name');
|
||||
|
||||
% Closed-Loop Plants - RDC
|
||||
G_vc_norot_rdc = feedback(G_vc_norot, Krdc_vc, 'name');
|
||||
G_vc_fast_rdc = feedback(G_vc_fast, Krdc_vc, 'name');
|
||||
|
||||
G_md_norot_rdc = feedback(G_md_norot, Krdc_md, 'name');
|
||||
G_md_fast_rdc = feedback(G_md_fast, Krdc_md, 'name');
|
||||
|
||||
G_pz_norot_rdc = feedback(G_pz_norot, Krdc_pz, 'name');
|
||||
G_pz_fast_rdc = feedback(G_pz_fast, Krdc_pz, 'name');
|
||||
|
||||
%% Bode plot of the transfer function from nano-hexapod actuator to measured motion by the external metrology
|
||||
freqs_vc = logspace(-1, 2, 1000);
|
||||
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast('Dx', 'Fu'), freqs_vc, 'Hz'))), 'color', zeros(1,3), ...
|
||||
'DisplayName', 'OL');
|
||||
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast('Dy', 'Fu'), freqs_vc, 'Hz'))), 'color', [zeros(1,3), 0.5], ...
|
||||
'DisplayName', 'Coupling');
|
||||
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_iff_hpf('Dx', 'Fu'), freqs_vc, 'Hz'))), 'color', colors(1,:), ...
|
||||
'DisplayName', 'IFF + $k_p$');
|
||||
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_iff_hpf('Dy', 'Fu'), freqs_vc, 'Hz'))), 'color', [colors(1,:), 0.5], ...
|
||||
'HandleVisibility', 'off');
|
||||
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_iff_kp('Dx', 'Fu'), freqs_vc, 'Hz'))), 'color', colors(2,:), ...
|
||||
'DisplayName', 'IFF + HPF');
|
||||
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_iff_kp('Dy', 'Fu'), freqs_vc, 'Hz'))), 'color', [colors(2,:), 0.5], ...
|
||||
'HandleVisibility', 'off');
|
||||
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_rdc('Dx', 'Fu'), freqs_vc, 'Hz'))), 'color', colors(3,:), ...
|
||||
'DisplayName', 'RDC');
|
||||
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_rdc('Dy', 'Fu'), freqs_vc, 'Hz'))), 'color', [colors(3,:), 0.5], ...
|
||||
'HandleVisibility', 'off');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
ylim([1e-8, 1e-2])
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
plot(freqs_vc, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_fast('Dx', 'Fu'), freqs_vc, 'Hz')))), 'color', zeros(1,3));
|
||||
plot(freqs_vc, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_fast_iff_hpf('Dx', 'Fu'), freqs_vc, 'Hz')))), 'color', colors(1,:));
|
||||
plot(freqs_vc, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_fast_iff_kp('Dx', 'Fu'), freqs_vc, 'Hz')))), 'color', colors(2,:));
|
||||
plot(freqs_vc, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_fast_rdc('Dx', 'Fu'), freqs_vc, 'Hz')))), 'color', colors(3,:));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||||
hold off;
|
||||
yticks(-360:90:360);
|
||||
ylim([ -200, 20]);
|
||||
|
||||
linkaxes([ax,ax2],'x');
|
||||
xlim([freqs_vc(1), freqs_vc(end)]);
|
||||
xticks([1e-1, 1e0, 1e1]);
|
||||
|
||||
freqs_md = logspace(0, 3, 1000);
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(freqs_md, abs(squeeze(freqresp(G_md_fast('Dx', 'Fu'), freqs_md, 'Hz'))), 'color', zeros(1,3), ...
|
||||
'DisplayName', 'OL');
|
||||
plot(freqs_md, abs(squeeze(freqresp(G_md_fast('Dy', 'Fu'), freqs_md, 'Hz'))), 'color', [zeros(1,3), 0.5], ...
|
||||
'DisplayName', 'Coupling');
|
||||
plot(freqs_md, abs(squeeze(freqresp(G_md_fast_iff_hpf('Dx', 'Fu'), freqs_md, 'Hz'))), 'color', colors(1,:), ...
|
||||
'DisplayName', 'IFF + $k_p$');
|
||||
plot(freqs_md, abs(squeeze(freqresp(G_md_fast_iff_hpf('Dy', 'Fu'), freqs_md, 'Hz'))), 'color', [colors(1,:), 0.5], ...
|
||||
'HandleVisibility', 'off');
|
||||
plot(freqs_md, abs(squeeze(freqresp(G_md_fast_iff_kp('Dx', 'Fu'), freqs_md, 'Hz'))), 'color', colors(2,:), ...
|
||||
'DisplayName', 'IFF + HPF');
|
||||
plot(freqs_md, abs(squeeze(freqresp(G_md_fast_iff_kp('Dy', 'Fu'), freqs_md, 'Hz'))), 'color', [colors(2,:), 0.5], ...
|
||||
'HandleVisibility', 'off');
|
||||
plot(freqs_md, abs(squeeze(freqresp(G_md_fast_rdc('Dx', 'Fu'), freqs_md, 'Hz'))), 'color', colors(3,:), ...
|
||||
'DisplayName', 'RDC');
|
||||
plot(freqs_md, abs(squeeze(freqresp(G_md_fast_rdc('Dy', 'Fu'), freqs_md, 'Hz'))), 'color', [colors(3,:), 0.5], ...
|
||||
'HandleVisibility', 'off');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
ylim([1e-10, 1e-4])
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
plot(freqs_md, 180/pi*unwrap(angle(squeeze(freqresp(G_md_fast('Dx', 'Fu'), freqs_md, 'Hz')))), 'color', zeros(1,3));
|
||||
plot(freqs_md, 180/pi*unwrap(angle(squeeze(freqresp(G_md_fast_iff_hpf('Dx', 'Fu'), freqs_md, 'Hz')))), 'color', colors(1,:));
|
||||
plot(freqs_md, 180/pi*unwrap(angle(squeeze(freqresp(G_md_fast_iff_kp('Dx', 'Fu'), freqs_md, 'Hz')))), 'color', colors(2,:));
|
||||
plot(freqs_md, 180/pi*unwrap(angle(squeeze(freqresp(G_md_fast_rdc('Dx', 'Fu'), freqs_md, 'Hz')))), 'color', colors(3,:));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||||
hold off;
|
||||
yticks(-360:90:360);
|
||||
ylim([ -200, 20]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([freqs_md(1), freqs_md(end)]);
|
||||
xticks([1e0, 1e1, 1e2]);
|
||||
|
||||
freqs_pz = logspace(0, 3, 1000);
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast('Dx', 'Fu'), freqs_pz, 'Hz'))), 'color', zeros(1,3), ...
|
||||
'DisplayName', 'OL');
|
||||
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast('Dy', 'Fu'), freqs_pz, 'Hz'))), 'color', [zeros(1,3), 0.5], ...
|
||||
'DisplayName', 'Coupling');
|
||||
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_iff_hpf('Dx', 'Fu'), freqs_pz, 'Hz'))), 'color', colors(1,:), ...
|
||||
'DisplayName', 'IFF + $k_p$');
|
||||
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_iff_hpf('Dy', 'Fu'), freqs_pz, 'Hz'))), 'color', [colors(1,:), 0.5], ...
|
||||
'HandleVisibility', 'off');
|
||||
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_iff_kp('Dx', 'Fu'), freqs_pz, 'Hz'))), 'color', colors(2,:), ...
|
||||
'DisplayName', 'IFF + HPF');
|
||||
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_iff_kp('Dy', 'Fu'), freqs_pz, 'Hz'))), 'color', [colors(2,:), 0.5], ...
|
||||
'HandleVisibility', 'off');
|
||||
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_rdc('Dx', 'Fu'), freqs_pz, 'Hz'))), 'color', colors(3,:), ...
|
||||
'DisplayName', 'RDC');
|
||||
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_rdc('Dy', 'Fu'), freqs_pz, 'Hz'))), 'color', [colors(3,:), 0.5], ...
|
||||
'HandleVisibility', 'off');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
ylim([1e-12, 1e-6])
|
||||
ldg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
||||
ldg.ItemTokenSize = [20, 1];
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
plot(freqs_pz, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_fast('Dx', 'Fu'), freqs_pz, 'Hz')))), 'color', zeros(1,3));
|
||||
plot(freqs_pz, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_fast_iff_hpf('Dx', 'Fu'), freqs_pz, 'Hz')))), 'color', colors(1,:));
|
||||
plot(freqs_pz, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_fast_iff_kp('Dx', 'Fu'), freqs_pz, 'Hz')))), 'color', colors(2,:));
|
||||
plot(freqs_pz, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_fast_rdc('Dx', 'Fu'), freqs_pz, 'Hz')))), 'color', colors(3,:));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||||
hold off;
|
||||
yticks(-360:90:360);
|
||||
ylim([ -200, 20]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([freqs_pz(1), freqs_pz(end)]);
|
||||
xticks([1e0, 1e1, 1e2]);
|
||||
|
||||
%% Effect of Floor motion on the position error - Comparison of active damping techniques for the three nano-hexapod stiffnesses
|
||||
freqs = logspace(-1, 3, 1000);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_fast('Dx', 'Dfx'), freqs, 'Hz'))), 'color', zeros(1,3), ...
|
||||
'DisplayName', 'OL');
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_fast_iff_hpf('Dx', 'Dfx'), freqs, 'Hz'))), 'color', colors(1,:), ...
|
||||
'DisplayName', 'IFF + $k_p$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_fast_iff_kp('Dx', 'Dfx'), freqs, 'Hz'))), 'color', colors(2,:), ...
|
||||
'DisplayName', 'IFF + HPF');
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_fast_rdc('Dx', 'Dfx'), freqs, 'Hz'))), 'color', colors(3,:), ...
|
||||
'DisplayName', 'RDC');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Magnitude $d_x/x_{f,x}$ [m/N]');
|
||||
xticks([1e-1, 1e0, 1e1, 1e2, 1e3]);
|
||||
xtickangle(0)
|
||||
ylim([1e-4, 1e2]);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_fast('Dx', 'Dfx'), freqs, 'Hz'))), 'color', zeros(1,3), ...
|
||||
'DisplayName', 'OL');
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_fast_iff_hpf('Dx', 'Dfx'), freqs, 'Hz'))), 'color', colors(1,:), ...
|
||||
'DisplayName', 'IFF + $k_p$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_fast_iff_kp('Dx', 'Dfx'), freqs, 'Hz'))), 'color', colors(2,:), ...
|
||||
'DisplayName', 'IFF + HPF');
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_fast_rdc('Dx', 'Dfx'), freqs, 'Hz'))), 'color', colors(3,:), ...
|
||||
'DisplayName', 'RDC');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Magnitude $d_x/x_{f,x}$ [m/N]');
|
||||
xticks([1e-1, 1e0, 1e1, 1e2, 1e3]);
|
||||
xtickangle(0)
|
||||
ylim([1e-4, 1e2]);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_fast('Dx', 'Dfx'), freqs, 'Hz'))), 'color', zeros(1,3), ...
|
||||
'DisplayName', 'OL');
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_fast_iff_hpf('Dx', 'Dfx'), freqs, 'Hz'))), 'color', colors(1,:), ...
|
||||
'DisplayName', 'IFF + $k_p$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_fast_iff_kp('Dx', 'Dfx'), freqs, 'Hz'))), 'color', colors(2,:), ...
|
||||
'DisplayName', 'IFF + HPF');
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_fast_rdc('Dx', 'Dfx'), freqs, 'Hz'))), 'color', colors(3,:), ...
|
||||
'DisplayName', 'RDC');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Magnitude $d_x/x_{f,x}$ [m/N]');
|
||||
xticks([1e-1, 1e0, 1e1, 1e2, 1e3]);
|
||||
xtickangle(0)
|
||||
ldg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
||||
ldg.ItemTokenSize = [15, 1];
|
||||
ylim([1e-4, 1e2]);
|
||||
|
||||
%% Effect of micro-station vibrations on the position error - Comparison of active damping techniques for the three nano-hexapod stiffnesses
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_fast('Dx', 'Ftx'), freqs, 'Hz'))), 'color', zeros(1,3), ...
|
||||
'DisplayName', 'OL');
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_fast_iff_hpf('Dx', 'Ftx'), freqs, 'Hz'))), 'color', colors(1,:), ...
|
||||
'DisplayName', 'IFF + $k_p$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_fast_iff_kp('Dx', 'Ftx'), freqs, 'Hz'))), 'color', colors(2,:), ...
|
||||
'DisplayName', 'IFF + HPF');
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_fast_rdc('Dx', 'Ftx'), freqs, 'Hz'))), 'color', colors(3,:), ...
|
||||
'DisplayName', 'RDC');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Magnitude $d_x/f_{t,x}$ [m/N]');
|
||||
xticks([1e-1, 1e0, 1e1, 1e2, 1e3]);
|
||||
xtickangle(0)
|
||||
ylim([1e-12, 2e-7]);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_fast('Dx', 'Ftx'), freqs, 'Hz'))), 'color', zeros(1,3), ...
|
||||
'DisplayName', 'OL');
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_fast_iff_hpf('Dx', 'Ftx'), freqs, 'Hz'))), 'color', colors(1,:), ...
|
||||
'DisplayName', 'IFF + $k_p$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_fast_iff_kp('Dx', 'Ftx'), freqs, 'Hz'))), 'color', colors(2,:), ...
|
||||
'DisplayName', 'IFF + HPF');
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_fast_rdc('Dx', 'Ftx'), freqs, 'Hz'))), 'color', colors(3,:), ...
|
||||
'DisplayName', 'RDC');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Magnitude $d_x/f_{t,x}$ [m/N]');
|
||||
xticks([1e-1, 1e0, 1e1, 1e2, 1e3]);
|
||||
xtickangle(0)
|
||||
ylim([1e-12, 2e-7]);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_fast('Dx', 'Ftx'), freqs, 'Hz'))), 'color', zeros(1,3), ...
|
||||
'DisplayName', 'OL');
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_fast_iff_hpf('Dx', 'Ftx'), freqs, 'Hz'))), 'color', colors(1,:), ...
|
||||
'DisplayName', 'IFF + $k_p$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_fast_iff_kp('Dx', 'Ftx'), freqs, 'Hz'))), 'color', colors(2,:), ...
|
||||
'DisplayName', 'IFF + HPF');
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_fast_rdc('Dx', 'Ftx'), freqs, 'Hz'))), 'color', colors(3,:), ...
|
||||
'DisplayName', 'RDC');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Magnitude $d_x/f_{t,x}$ [m/N]');
|
||||
xticks([1e-1, 1e0, 1e1, 1e2, 1e3]);
|
||||
xtickangle(0)
|
||||
ldg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
||||
ldg.ItemTokenSize = [20, 1];
|
||||
ylim([1e-12, 2e-7]);
|
||||
|
||||
%% Effect of sample forces on the position error - Comparison of active damping techniques for the three nano-hexapod stiffnesses
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_fast('Dx', 'Fdx'), freqs, 'Hz'))), 'color', zeros(1,3), ...
|
||||
'DisplayName', 'OL');
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_fast_iff_hpf('Dx', 'Fdx'), freqs, 'Hz'))), 'color', colors(1,:), ...
|
||||
'DisplayName', 'IFF + $k_p$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_fast_iff_kp('Dx', 'Fdx'), freqs, 'Hz'))), 'color', colors(2,:), ...
|
||||
'DisplayName', 'IFF + HPF');
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_fast_rdc('Dx', 'Fdx'), freqs, 'Hz'))), 'color', colors(3,:), ...
|
||||
'DisplayName', 'RDC');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Magnitude $d_x/f_{s,x}$ [m/N]');
|
||||
xticks([1e-1, 1e0, 1e1, 1e2, 1e3]);
|
||||
xtickangle(0)
|
||||
ylim([1e-8, 1e-2])
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_fast('Dx', 'Fdx'), freqs, 'Hz'))), 'color', zeros(1,3), ...
|
||||
'DisplayName', 'OL');
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_fast_iff_hpf('Dx', 'Fdx'), freqs, 'Hz'))), 'color', colors(1,:), ...
|
||||
'DisplayName', 'IFF + $k_p$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_fast_iff_kp('Dx', 'Fdx'), freqs, 'Hz'))), 'color', colors(2,:), ...
|
||||
'DisplayName', 'IFF + HPF');
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_fast_rdc('Dx', 'Fdx'), freqs, 'Hz'))), 'color', colors(3,:), ...
|
||||
'DisplayName', 'RDC');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Magnitude $d_x/f_{s,x}$ [m/N]');
|
||||
xticks([1e-1, 1e0, 1e1, 1e2, 1e3]);
|
||||
xtickangle(0)
|
||||
ylim([1e-8, 1e-2])
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_fast('Dx', 'Fdx'), freqs, 'Hz'))), 'color', zeros(1,3), ...
|
||||
'DisplayName', 'OL');
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_fast_iff_hpf('Dx', 'Fdx'), freqs, 'Hz'))), 'color', colors(1,:), ...
|
||||
'DisplayName', 'IFF + $k_p$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_fast_iff_kp('Dx', 'Fdx'), freqs, 'Hz'))), 'color', colors(2,:), ...
|
||||
'DisplayName', 'IFF + HPF');
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_fast_rdc('Dx', 'Fdx'), freqs, 'Hz'))), 'color', colors(3,:), ...
|
||||
'DisplayName', 'RDC');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Magnitude $d_x/f_{s,x}$ [m/N]');
|
||||
xticks([1e-1, 1e0, 1e1, 1e2, 1e3]);
|
||||
xtickangle(0)
|
||||
ldg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
||||
ldg.ItemTokenSize = [20, 1];
|
||||
|
||||
linkaxes([ax1,ax2,ax3], 'y')
|
||||
ylim([1e-8, 1e-2])
|
||||
BIN
A2-nass-rotating-3dof-model/rotating_model.slx
Normal file
BIN
A2-nass-rotating-3dof-model/rotating_model.slx
Normal file
Binary file not shown.
@ -0,0 +1,8 @@
|
||||
function [xi_min] = computeSimultaneousDamping(g, G, K)
|
||||
[~, xi] = damp(minreal(feedback(G, g*K), [], false));
|
||||
xi_min = 1/min(xi);
|
||||
|
||||
if xi_min < 0
|
||||
xi_min = 1e8;
|
||||
end
|
||||
end
|
||||
94
A2-nass-rotating-3dof-model/src/rootLocusPolesSorted.m
Normal file
94
A2-nass-rotating-3dof-model/src/rootLocusPolesSorted.m
Normal file
@ -0,0 +1,94 @@
|
||||
function [poles] = rootLocusPolesSorted(G, K, gains, args)
|
||||
% rootLocusPolesSorted -
|
||||
%
|
||||
% Syntax: [poles] = rootLocusPolesSorted(G, K, gains, args)
|
||||
%
|
||||
% Inputs:
|
||||
% - G, K, gains, args -
|
||||
%
|
||||
% Outputs:
|
||||
% - poles -
|
||||
|
||||
arguments
|
||||
G
|
||||
K
|
||||
gains
|
||||
args.minreal double {mustBeNumericOrLogical} = false
|
||||
args.p_half double {mustBeNumericOrLogical} = false
|
||||
args.d_max double {mustBeNumeric} = -1
|
||||
end
|
||||
|
||||
if args.minreal
|
||||
p1 = pole(minreal(feedback(G, gains(1)*K)));
|
||||
[~, i_uniq] = uniquetol([real(p1), imag(p1)], 1e-10, 'ByRows', true);
|
||||
p1 = p1(i_uniq);
|
||||
|
||||
poles = zeros(length(p1), length(gains));
|
||||
poles(:, 1) = p1;
|
||||
else
|
||||
p1 = pole(feedback(G, gains(1)*K));
|
||||
[~, i_uniq] = uniquetol([real(p1), imag(p1)], 1e-10, 'ByRows', true);
|
||||
p1 = p1(i_uniq);
|
||||
|
||||
poles = zeros(length(p1), length(gains));
|
||||
poles(:, 1) = p1;
|
||||
end
|
||||
|
||||
if args.minreal
|
||||
p2 = pole(minreal(feedback(G, gains(2)*K)));
|
||||
[~, i_uniq] = uniquetol([real(p2), imag(p2)], 1e-10, 'ByRows', true);
|
||||
p2 = p2(i_uniq);
|
||||
poles(:, 2) = p2;
|
||||
else
|
||||
p2 = pole(feedback(G, gains(2)*K));
|
||||
[~, i_uniq] = uniquetol([real(p2), imag(p2)], 1e-10, 'ByRows', true);
|
||||
p2 = p2(i_uniq);
|
||||
poles(:, 2) = p2;
|
||||
end
|
||||
|
||||
for g_i = 3:length(gains)
|
||||
% Estimated value of the poles
|
||||
poles_est = poles(:, g_i-1) + (poles(:, g_i-1) - poles(:, g_i-2))*(gains(g_i) - gains(g_i-1))/(gains(g_i-1) - gains(g_i - 2));
|
||||
|
||||
% New values for the poles
|
||||
poles_gi = pole(feedback(G, gains(g_i)*K));
|
||||
[~, i_uniq] = uniquetol([real(poles_gi), imag(poles_gi)], 1e-10, 'ByRows', true);
|
||||
poles_gi = poles_gi(i_uniq);
|
||||
|
||||
% Array of distances between all the poles
|
||||
poles_dist = sqrt((poles_est-poles_gi.').*conj(poles_est-poles_gi.'));
|
||||
|
||||
% Get indices corresponding to distances from lowest to highest
|
||||
[~, c] = sort(min(poles_dist));
|
||||
|
||||
as = 1:length(poles_gi);
|
||||
|
||||
% for each column of poles_dist corresponding to the i'th pole
|
||||
% with closest previous poles
|
||||
for p_i = c
|
||||
% Get the indice a_i of the previous pole that is the closest
|
||||
% to pole c(p_i)
|
||||
[~, a_i] = min(poles_dist(:, p_i));
|
||||
|
||||
poles(as(a_i), g_i) = poles_gi(p_i);
|
||||
|
||||
% Remove old poles that are already matched
|
||||
% poles_gi(as(a_i), :) = [];
|
||||
poles_dist(a_i, :) = [];
|
||||
as(a_i) = [];
|
||||
end
|
||||
end
|
||||
|
||||
|
||||
if args.d_max > 0
|
||||
poles = poles(max(abs(poles(:, 2:end) - poles(:, 1:end-1))') > args.d_max, :);
|
||||
end
|
||||
|
||||
if args.p_half
|
||||
poles = poles(1:round(end/2), :);
|
||||
end
|
||||
|
||||
[~, s_p] = sort(imag(poles(:,1)), 'descend');
|
||||
poles = poles(s_p, :);
|
||||
|
||||
poles = poles.';
|
||||
23
A3-micro-station-modal-analysis/mat/acc_pos.txt
Normal file
23
A3-micro-station-modal-analysis/mat/acc_pos.txt
Normal file
@ -0,0 +1,23 @@
|
||||
23 1.5500e-001 -9.0000e-002 -5.9400e-001
|
||||
22 0.0000e+000 1.8000e-001 -5.9400e-001
|
||||
21 -1.5500e-001 -9.0000e-002 -5.9400e-001
|
||||
20 8.6490e-001 -5.0600e-001 -9.5060e-001
|
||||
19 8.7500e-001 7.9900e-001 -9.5060e-001
|
||||
18 -7.3500e-001 8.1400e-001 -9.5060e-001
|
||||
17 -7.3000e-001 -5.2600e-001 -9.5060e-001
|
||||
16 2.9500e-001 -4.8100e-001 -7.8560e-001
|
||||
15 4.5000e-001 5.3400e-001 -7.8560e-001
|
||||
14 -4.8000e-001 5.3400e-001 -7.8560e-001
|
||||
13 -3.2000e-001 -4.4600e-001 -7.8560e-001
|
||||
12 4.7500e-001 -4.1900e-001 -4.2730e-001
|
||||
11 4.7500e-001 4.2400e-001 -4.2730e-001
|
||||
10 -4.6500e-001 4.0700e-001 -4.2730e-001
|
||||
9 -4.7500e-001 -4.1400e-001 -4.2730e-001
|
||||
8 3.8000e-001 -3.0000e-001 -4.1680e-001
|
||||
7 4.2000e-001 2.8000e-001 -4.1680e-001
|
||||
6 -4.2000e-001 2.8000e-001 -4.1680e-001
|
||||
5 -3.8500e-001 -3.0000e-001 -4.1680e-001
|
||||
4 6.4000e-002 -6.4000e-002 -2.7000e-001
|
||||
3 6.4000e-002 6.4000e-002 -2.7000e-001
|
||||
2 -6.4000e-002 6.4000e-002 -2.7000e-001
|
||||
1 -6.4000e-002 -6.4000e-002 -2.7000e-001
|
||||
BIN
A3-micro-station-modal-analysis/mat/frf_com.mat
(Stored with Git LFS)
Normal file
BIN
A3-micro-station-modal-analysis/mat/frf_com.mat
(Stored with Git LFS)
Normal file
Binary file not shown.
BIN
A3-micro-station-modal-analysis/mat/frf_matrix.mat
(Stored with Git LFS)
Normal file
BIN
A3-micro-station-modal-analysis/mat/frf_matrix.mat
(Stored with Git LFS)
Normal file
Binary file not shown.
BIN
A3-micro-station-modal-analysis/mat/geometry.mat
(Stored with Git LFS)
Normal file
BIN
A3-micro-station-modal-analysis/mat/geometry.mat
(Stored with Git LFS)
Normal file
Binary file not shown.
BIN
A3-micro-station-modal-analysis/mat/meas_microstation_frf.mat
(Stored with Git LFS)
Normal file
BIN
A3-micro-station-modal-analysis/mat/meas_microstation_frf.mat
(Stored with Git LFS)
Normal file
Binary file not shown.
BIN
A3-micro-station-modal-analysis/mat/meas_raw_1.mat
(Stored with Git LFS)
Normal file
BIN
A3-micro-station-modal-analysis/mat/meas_raw_1.mat
(Stored with Git LFS)
Normal file
Binary file not shown.
BIN
A3-micro-station-modal-analysis/mat/modal_microstation_compliance.mat
(Stored with Git LFS)
Normal file
BIN
A3-micro-station-modal-analysis/mat/modal_microstation_compliance.mat
(Stored with Git LFS)
Normal file
Binary file not shown.
16
A3-micro-station-modal-analysis/mat/mode_damps.txt
Normal file
16
A3-micro-station-modal-analysis/mat/mode_damps.txt
Normal file
@ -0,0 +1,16 @@
|
||||
12.20318
|
||||
11.66888
|
||||
6.19561
|
||||
2.79104
|
||||
2.76253
|
||||
4.34928
|
||||
1.25546
|
||||
3.65470
|
||||
2.94088
|
||||
3.19084
|
||||
1.55526
|
||||
3.13166
|
||||
2.76141
|
||||
1.34304
|
||||
2.43201
|
||||
1.38400
|
||||
16
A3-micro-station-modal-analysis/mat/mode_freqs.txt
Normal file
16
A3-micro-station-modal-analysis/mat/mode_freqs.txt
Normal file
@ -0,0 +1,16 @@
|
||||
11.86509
|
||||
18.55747
|
||||
37.82163
|
||||
39.07850
|
||||
56.31944
|
||||
69.78452
|
||||
72.49325
|
||||
84.83446
|
||||
91.26350
|
||||
105.47266
|
||||
106.57165
|
||||
112.67669
|
||||
124.20538
|
||||
145.30034
|
||||
150.52113
|
||||
165.42632
|
||||
16
A3-micro-station-modal-analysis/mat/mode_modal_a.txt
Normal file
16
A3-micro-station-modal-analysis/mat/mode_modal_a.txt
Normal file
@ -0,0 +1,16 @@
|
||||
4.13559e+003 +6.22828e+003
|
||||
2.76278e+002 +1.74197e+004
|
||||
-1.32270e+004 +2.17346e+004
|
||||
-2.48397e+005 -1.60998e+005
|
||||
-4.23967e+004 +7.06852e+004
|
||||
-7.36964e+003 +4.57024e+004
|
||||
1.37806e+005 +3.00336e+005
|
||||
-1.31109e+004 +2.81759e+004
|
||||
5.59259e+003 -4.27543e+004
|
||||
-5.28869e+004 +6.38436e+003
|
||||
3.71578e+004 +1.57745e+004
|
||||
-4.24659e+004 +7.90956e+003
|
||||
-3.57355e+004 +1.13161e+005
|
||||
5.24764e+004 -1.45211e+005
|
||||
1.97228e+005 +2.51758e+005
|
||||
-3.00273e+005 +3.27201e+005
|
||||
16
A3-micro-station-modal-analysis/mat/mode_modal_b.txt
Normal file
16
A3-micro-station-modal-analysis/mat/mode_modal_b.txt
Normal file
@ -0,0 +1,16 @@
|
||||
4.98475e+005 -2.49344e+005
|
||||
2.02102e+006 +2.05017e+005
|
||||
4.96035e+006 +3.45724e+006
|
||||
-4.12180e+007 +5.98638e+007
|
||||
2.45891e+007 +1.56880e+007
|
||||
1.98796e+007 +4.09986e+006
|
||||
1.37577e+008 -6.10466e+007
|
||||
1.47532e+007 +7.53272e+006
|
||||
-2.44115e+007 -3.92655e+006
|
||||
3.11045e+006 +3.51656e+007
|
||||
1.09485e+007 -2.47140e+007
|
||||
4.65546e+006 +3.02251e+007
|
||||
8.75076e+007 +3.03162e+007
|
||||
-1.31915e+008 -4.96844e+007
|
||||
2.42567e+008 -1.80683e+008
|
||||
3.35742e+008 +3.16782e+008
|
||||
1104
A3-micro-station-modal-analysis/mat/mode_shapes.txt
Normal file
1104
A3-micro-station-modal-analysis/mat/mode_shapes.txt
Normal file
File diff suppressed because it is too large
Load Diff
18
A3-micro-station-modal-analysis/mat/model_solidworks_com.txt
Normal file
18
A3-micro-station-modal-analysis/mat/model_solidworks_com.txt
Normal file
@ -0,0 +1,18 @@
|
||||
0.045
|
||||
0.144
|
||||
-1.251
|
||||
0.052
|
||||
0.258
|
||||
-0.778
|
||||
0.000
|
||||
0.014
|
||||
-0.600
|
||||
0.000
|
||||
-0.005
|
||||
-0.628
|
||||
0.000
|
||||
0.000
|
||||
-0.580
|
||||
-0.004
|
||||
0.006
|
||||
-0.319
|
||||
83
A3-micro-station-modal-analysis/modal_1_meas_setup.m
Normal file
83
A3-micro-station-modal-analysis/modal_1_meas_setup.m
Normal file
@ -0,0 +1,83 @@
|
||||
%% Clear Workspace and Close figures
|
||||
clear; close all; clc;
|
||||
|
||||
%% Intialize Laplace variable
|
||||
s = zpk('s');
|
||||
|
||||
%% Path for functions, data and scripts
|
||||
addpath('./mat/'); % Path for data
|
||||
|
||||
%% Colors for the figures
|
||||
colors = colororder;
|
||||
|
||||
%% Load Accelerometer positions
|
||||
acc_pos = readtable('mat/acc_pos.txt', 'ReadVariableNames', false);
|
||||
acc_pos = table2array(acc_pos(:, 1:4));
|
||||
[~, i] = sort(acc_pos(:, 1));
|
||||
acc_pos = acc_pos(i, 2:4);
|
||||
|
||||
%% Load raw data
|
||||
meas1_raw = load('mat/meas_raw_1.mat');
|
||||
|
||||
% Sampling Frequency [Hz]
|
||||
Fs = 1/meas1_raw.Track1_X_Resolution;
|
||||
|
||||
% Time just before the impact occurs [s]
|
||||
impacts = [5.937, 11.228, 16.681, 22.205, 27.350, 32.714, 38.115, 43.888, 50.407]-0.01;
|
||||
|
||||
% Time vector [s]
|
||||
time = linspace(0, meas1_raw.Track1_X_Resolution*length(meas1_raw.Track1), length(meas1_raw.Track1));
|
||||
|
||||
%% Raw measurement of the Accelerometer
|
||||
figure;
|
||||
hold on;
|
||||
plot(time-22.2, meas1_raw.Track2, 'DisplayName', '$X_{1,x}$ [$m/s^2$]');
|
||||
plot(time-22.2, 1e-3*meas1_raw.Track1, 'DisplayName', '$F_{z}$ [kN]');
|
||||
hold off;
|
||||
xlabel('Time [s]');
|
||||
ylabel('Amplitude');
|
||||
xlim([0, 0.2])
|
||||
ylim([-2, 2]);
|
||||
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
||||
|
||||
%% Frequency Analysis
|
||||
Nfft = floor(5.0*Fs); % Number of frequency points
|
||||
win = hanning(Nfft); % Windowing
|
||||
Noverlap = floor(Nfft/2); % Overlap for frequency analysis
|
||||
|
||||
%% Comnpute the power spectral density of the force and acceleration
|
||||
[pxx_force, f] = pwelch(meas1_raw.Track1, win, Noverlap, Nfft, Fs);
|
||||
[pxx_acc, ~] = pwelch(meas1_raw.Track2, win, Noverlap, Nfft, Fs);
|
||||
|
||||
%% Normalized Amplitude Spectral Density of the measured force and acceleration
|
||||
figure;
|
||||
hold on;
|
||||
plot(f, sqrt(pxx_acc./max(pxx_acc(f<200))), 'DisplayName', '$\Gamma_{X_{1,x}}$');
|
||||
plot(f, sqrt(pxx_force./max(pxx_force(f<200))), 'DisplayName', '$\Gamma_{F_{z}}$');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Frequency [Hz]'); ylabel('Normalized Spectral Density');
|
||||
xlim([0, 200]);
|
||||
xticks([0:20:200]);
|
||||
ylim([0, 1])
|
||||
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
||||
|
||||
%% Compute the transfer function and Coherence
|
||||
[G1, f] = tfestimate(meas1_raw.Track1, meas1_raw.Track2, win, Noverlap, Nfft, Fs);
|
||||
[coh1, ~] = mscohere( meas1_raw.Track1, meas1_raw.Track2, win, Noverlap, Nfft, Fs);
|
||||
|
||||
%% Frequency Response Function between the force and the acceleration
|
||||
figure;
|
||||
plot(f, abs(G1));
|
||||
xlabel('Frequency [Hz]'); ylabel('FRF [$m/s^2/N$]')
|
||||
set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'log');
|
||||
xlim([0, 200]);
|
||||
xticks([0:20:200]);
|
||||
|
||||
%% Frequency Response Function between the force and the acceleration
|
||||
figure;
|
||||
plot(f, coh1);
|
||||
xlabel('Frequency [Hz]'); ylabel('Coherence [-]')
|
||||
set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
|
||||
xlim([0, 200]); ylim([0,1]);
|
||||
xticks([0:20:200]);
|
||||
132
A3-micro-station-modal-analysis/modal_2_frf_processing.m
Normal file
132
A3-micro-station-modal-analysis/modal_2_frf_processing.m
Normal file
@ -0,0 +1,132 @@
|
||||
%% Clear Workspace and Close figures
|
||||
clear; close all; clc;
|
||||
|
||||
%% Intialize Laplace variable
|
||||
s = zpk('s');
|
||||
|
||||
%% Path for functions, data and scripts
|
||||
addpath('./mat/'); % Path for data
|
||||
|
||||
%% Colors for the figures
|
||||
colors = colororder;
|
||||
|
||||
%% Load frequency response matrix
|
||||
load('frf_matrix.mat', 'freqs', 'frf');
|
||||
|
||||
%% Load Accelerometer positions
|
||||
acc_pos = readtable('mat/acc_pos.txt', 'ReadVariableNames', false);
|
||||
acc_pos = table2array(acc_pos(:, 1:4));
|
||||
[~, i] = sort(acc_pos(:, 1));
|
||||
acc_pos = acc_pos(i, 2:4);
|
||||
|
||||
%% Accelerometers ID connected to each solid body
|
||||
solids = {};
|
||||
solids.gbot = [17, 18, 19, 20]; % bottom granite
|
||||
solids.gtop = [13, 14, 15, 16]; % top granite
|
||||
solids.ty = [9, 10, 11, 12]; % Ty stage
|
||||
solids.ry = [5, 6, 7, 8]; % Ry stage
|
||||
solids.rz = [21, 22, 23]; % Rz stage
|
||||
solids.hexa = [1, 2, 3, 4]; % Hexapod
|
||||
|
||||
% Names of the solid bodies
|
||||
solid_names = fields(solids);
|
||||
|
||||
%% Save the accelerometer positions are well as the solid bodies
|
||||
save('mat/geometry.mat', 'solids', 'solid_names', 'acc_pos');
|
||||
|
||||
%% Extract the CoM of considered solid bodies
|
||||
model_com = reshape(table2array(readtable('mat/model_solidworks_com.txt', 'ReadVariableNames', false)), [3, 6]);
|
||||
|
||||
%% Frequency Response Matrix - Response expressed at the CoM of the solid bodies
|
||||
frfs_CoM = zeros(length(solid_names)*6, 3, 801);
|
||||
|
||||
for solid_i = 1:length(solid_names)
|
||||
% Number of accelerometers fixed to this solid body
|
||||
solids_i = solids.(solid_names{solid_i});
|
||||
|
||||
% "Jacobian" matrix to go from accelerometer frame to CoM frame
|
||||
A = zeros(3*length(solids_i), 6);
|
||||
for i = 1:length(solids_i)
|
||||
acc_i = solids_i(i);
|
||||
|
||||
acc_pos_com = acc_pos(acc_i, :).' - model_com(:, solid_i);
|
||||
|
||||
A(3*(i-1)+1:3*i, 1:3) = eye(3);
|
||||
A(3*(i-1)+1:3*i, 4:6) = [ 0 acc_pos_com(3) -acc_pos_com(2) ;
|
||||
-acc_pos_com(3) 0 acc_pos_com(1) ;
|
||||
acc_pos_com(2) -acc_pos_com(1) 0];
|
||||
end
|
||||
|
||||
for exc_dir = 1:3
|
||||
frfs_CoM((solid_i-1)*6+1:solid_i*6, exc_dir, :) = A\squeeze(frf((solids_i(1)-1)*3+1:solids_i(end)*3, exc_dir, :));
|
||||
end
|
||||
end
|
||||
|
||||
%% Save the computed FRF at the CoM
|
||||
save('mat/frf_com.mat', 'frfs_CoM');
|
||||
|
||||
%% Compute the FRF at the accelerometer location from the CoM reponses
|
||||
frfs_A = zeros(size(frf));
|
||||
|
||||
% For each excitation direction
|
||||
for exc_dir = 1:3
|
||||
% For each solid
|
||||
for solid_i = 1:length(solid_names)
|
||||
v0 = squeeze(frfs_CoM((solid_i-1)*6+1:(solid_i-1)*6+3, exc_dir, :));
|
||||
W0 = squeeze(frfs_CoM((solid_i-1)*6+4:(solid_i-1)*6+6, exc_dir, :));
|
||||
|
||||
% For each accelerometer attached to the current solid
|
||||
for acc_i = solids.(solid_names{solid_i})
|
||||
% We get the position of the accelerometer expressed in frame O
|
||||
pos = acc_pos(acc_i, :).' - model_com(:, solid_i);
|
||||
% pos = acc_pos(acc_i, :).';
|
||||
posX = [0 pos(3) -pos(2); -pos(3) 0 pos(1) ; pos(2) -pos(1) 0];
|
||||
|
||||
frfs_A(3*(acc_i-1)+1:3*(acc_i-1)+3, exc_dir, :) = v0 + posX*W0;
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
%% Comparison of the original accelerometer response and reconstructed response from the solid body response
|
||||
exc_names = {'$F_x$', '$F_y$', '$F_z$'};
|
||||
DOFs = {'x', 'y', 'z', '\theta_x', '\theta_y', '\theta_z'};
|
||||
|
||||
solid_i = 6; % Considered solid body
|
||||
exc_dir = 1; % Excited direction
|
||||
|
||||
accs_i = solids.(solid_names{solid_i}); % Accelerometers fixed to this solid body
|
||||
|
||||
figure;
|
||||
tiledlayout(2, 2, 'TileSpacing', 'Tight', 'Padding', 'None');
|
||||
|
||||
for i = 1:length(accs_i)
|
||||
acc_i = accs_i(i);
|
||||
nexttile();
|
||||
|
||||
hold on;
|
||||
for dir_i = 1:3
|
||||
plot(freqs, abs(squeeze(frf(3*(acc_i-1)+dir_i, exc_dir, :))), '-', 'color', [colors(dir_i,:), 0.5], 'linewidth', 2.5, 'DisplayName', sprintf('$a_{%i,%s}$ - meas', acc_i, DOFs{dir_i}));
|
||||
end
|
||||
for dir_i = 1:3
|
||||
plot(freqs, abs(squeeze(frfs_A(3*(acc_i-1)+dir_i, exc_dir, :))), '-', 'color', colors(dir_i, :), 'DisplayName', sprintf('$a_{%i,%s}$ - solid body', acc_i, DOFs{dir_i}));
|
||||
end
|
||||
hold off;
|
||||
|
||||
if i > 2
|
||||
xlabel('Frequency [Hz]');
|
||||
else
|
||||
set(gca, 'XTickLabel',[]);
|
||||
end
|
||||
|
||||
if rem(i, 2) == 1
|
||||
ylabel('Amplitude [$\frac{m/s^2}{N}$]');
|
||||
else
|
||||
set(gca, 'YTickLabel',[]);
|
||||
end
|
||||
|
||||
set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'log');
|
||||
xlim([0, 200]); ylim([1e-6, 3e-2]);
|
||||
xticks([0:20:200]);
|
||||
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
|
||||
leg.ItemTokenSize(1) = 15;
|
||||
end
|
||||
151
A3-micro-station-modal-analysis/modal_3_analysis.m
Normal file
151
A3-micro-station-modal-analysis/modal_3_analysis.m
Normal file
@ -0,0 +1,151 @@
|
||||
%% Clear Workspace and Close figures
|
||||
clear; close all; clc;
|
||||
|
||||
%% Intialize Laplace variable
|
||||
s = zpk('s');
|
||||
|
||||
%% Path for functions, data and scripts
|
||||
addpath('./mat/'); % Path for data
|
||||
|
||||
%% Colors for the figures
|
||||
colors = colororder;
|
||||
|
||||
%% Load frequency response matrix
|
||||
load('frf_matrix.mat', 'freqs', 'frf');
|
||||
|
||||
%% Computation of the modal indication function
|
||||
MIF = zeros(size(frf, 2), size(frf, 2), size(frf, 3));
|
||||
|
||||
for i = 1:length(freqs)
|
||||
[~,S,~] = svd(frf(:, :, i));
|
||||
MIF(:, :, i) = S'*S;
|
||||
end
|
||||
|
||||
%% Modal Indication Function
|
||||
figure;
|
||||
hold on;
|
||||
for i = 1:size(MIF, 1)
|
||||
plot(freqs, squeeze(MIF(i, i, :)), 'DisplayName', sprintf('MIF${}_%i$', i));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'Xscale', 'lin'); set(gca, 'Yscale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('CMIF Amplitude');
|
||||
xticks([0:20:200]);
|
||||
xlim([0, 200]);
|
||||
ylim([1e-6, 2e-2]);
|
||||
ldg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
||||
|
||||
%% Load modal parameters
|
||||
shapes_m = readtable('mat/mode_shapes.txt', 'ReadVariableNames', false); % [Sign / Real / Imag]
|
||||
freqs_m = table2array(readtable('mat/mode_freqs.txt', 'ReadVariableNames', false)); % in [Hz]
|
||||
damps_m = table2array(readtable('mat/mode_damps.txt', 'ReadVariableNames', false)); % in [%]
|
||||
modal_a = table2array(readtable('mat/mode_modal_a.txt', 'ReadVariableNames', false)); % [Real / Imag]
|
||||
modal_b = table2array(readtable('mat/mode_modal_b.txt', 'ReadVariableNames', false)); % [Real / Imag]
|
||||
|
||||
%% Guess the number of modes identified from the length of the imported data.
|
||||
acc_n = 23; % Number of accelerometers
|
||||
dir_n = 3; % Number of directions
|
||||
dirs = 'XYZ';
|
||||
|
||||
mod_n = size(shapes_m,1)/acc_n/dir_n; % Number of modes
|
||||
|
||||
%% Mode shapes are split into 3 parts (direction plus sign, real part and imaginary part)
|
||||
% we aggregate them into one array of complex numbers
|
||||
T_sign = table2array(shapes_m(:, 1));
|
||||
T_real = table2array(shapes_m(:, 2));
|
||||
T_imag = table2array(shapes_m(:, 3));
|
||||
|
||||
mode_shapes = zeros(mod_n, dir_n, acc_n);
|
||||
|
||||
for mod_i = 1:mod_n
|
||||
for acc_i = 1:acc_n
|
||||
% Get the correct section of the signs
|
||||
T = T_sign(acc_n*dir_n*(mod_i-1)+1:acc_n*dir_n*mod_i);
|
||||
for dir_i = 1:dir_n
|
||||
% Get the line corresponding to the sensor
|
||||
i = find(contains(T, sprintf('%i%s',acc_i, dirs(dir_i))), 1, 'first')+acc_n*dir_n*(mod_i-1);
|
||||
mode_shapes(mod_i, dir_i, acc_i) = str2num([T_sign{i}(end-1), '1'])*complex(T_real(i),T_imag(i));
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
%% Create the eigenvalue and eigenvector matrices
|
||||
eigen_val_M = diag(2*pi*freqs_m.*(-damps_m/100 + j*sqrt(1 - (damps_m/100).^2))); % Lambda = diagonal matrix
|
||||
eigen_vec_M = reshape(mode_shapes, [mod_n, acc_n*dir_n]).'; % Phi, vecnorm(eigen_vec_M) = 1
|
||||
|
||||
% Add complex conjugate eigenvalues and eigenvectors
|
||||
eigen_val_ext_M = blkdiag(eigen_val_M, conj(eigen_val_M));
|
||||
eigen_vec_ext_M = [eigen_vec_M, conj(eigen_vec_M)];
|
||||
|
||||
%% "Modal A" and "Modal B" matrices
|
||||
modal_a_M = diag(complex(modal_a(:, 1), modal_a(:, 2)));
|
||||
modal_b_M = diag(complex(modal_b(:, 1), modal_b(:, 2)));
|
||||
|
||||
modal_a_ext_M = blkdiag(modal_a_M, conj(modal_a_M));
|
||||
modal_b_ext_M = blkdiag(modal_b_M, conj(modal_b_M));
|
||||
|
||||
%% Synthesize the full FRF matrix from the modal model
|
||||
Hsyn = zeros(acc_n*dir_n, acc_n*dir_n, length(freqs));
|
||||
|
||||
for i = 1:length(freqs)
|
||||
Hsyn(:, :, i) = eigen_vec_ext_M*diag(1./(diag(modal_a_ext_M).*(j*2*pi*freqs(i) - diag(eigen_val_ext_M))))*eigen_vec_ext_M.';
|
||||
end
|
||||
|
||||
%% Derivate two times to have the acceleration response
|
||||
for i = 1:size(Hsyn, 1)
|
||||
Hsyn(i, :, :) = squeeze(Hsyn(i, :, :)).*(j*2*pi*freqs).^2;
|
||||
end
|
||||
|
||||
acc_o = 11; dir_o = 3;
|
||||
acc_i = 11; dir_i = 3;
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(frf( 3*(acc_o-1)+dir_o, dir_i, :))), 'DisplayName', 'Measured');
|
||||
plot(freqs, abs(squeeze(Hsyn(3*(acc_o-1)+dir_o, 3*(acc_i-1)+dir_i, :))), 'DisplayName', 'Synthesized');
|
||||
hold off;
|
||||
set(gca, 'xscale', 'lin');
|
||||
set(gca, 'yscale', 'log');
|
||||
xlabel('Frequency [Hz]');
|
||||
ylabel('Magnitude [$\frac{m/s^2}{N}$]');
|
||||
ldg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
||||
ldg.ItemTokenSize = [10, 1];
|
||||
xticks([0:40:200]);
|
||||
xlim([1, 200]);
|
||||
ylim([1e-6, 1e-1]);
|
||||
|
||||
acc_o = 15; dir_o = 3;
|
||||
acc_i = 11; dir_i = 3;
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(frf( 3*(acc_o-1)+dir_o, dir_i, :))), 'DisplayName', 'Measured');
|
||||
plot(freqs, abs(squeeze(Hsyn(3*(acc_o-1)+dir_o, 3*(acc_i-1)+dir_i, :))), 'DisplayName', 'Synthesized');
|
||||
hold off;
|
||||
set(gca, 'xscale', 'lin');
|
||||
set(gca, 'yscale', 'log');
|
||||
xlabel('Frequency [Hz]');
|
||||
ylabel('Magnitude [$\frac{m/s^2}{N}$]');
|
||||
ldg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
||||
ldg.ItemTokenSize = [10, 1];
|
||||
xticks([0:40:200]);
|
||||
xlim([1, 200]);
|
||||
ylim([1e-6, 1e-1]);
|
||||
|
||||
acc_o = 2; dir_o = 1;
|
||||
acc_i = 11; dir_i = 2;
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(frf( 3*(acc_o-1)+dir_o, dir_i, :))), 'DisplayName', 'Measured');
|
||||
plot(freqs, abs(squeeze(Hsyn(3*(acc_o-1)+dir_o, 3*(acc_i-1)+dir_i, :))), 'DisplayName', 'Synthesized');
|
||||
hold off;
|
||||
set(gca, 'xscale', 'lin');
|
||||
set(gca, 'yscale', 'log');
|
||||
xlabel('Frequency [Hz]');
|
||||
ylabel('Magnitude [$\frac{m/s^2}{N}$]');
|
||||
ldg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
||||
ldg.ItemTokenSize = [10, 1];
|
||||
xticks([0:40:200]);
|
||||
xlim([1, 200]);
|
||||
ylim([1e-6, 1e-1]);
|
||||
49610
A4-simscape-micro-station/STEPS/Spindle_Rotor.STEP
Normal file
49610
A4-simscape-micro-station/STEPS/Spindle_Rotor.STEP
Normal file
File diff suppressed because it is too large
Load Diff
251845
A4-simscape-micro-station/STEPS/Spindle_Slip_Ring.STEP
Normal file
251845
A4-simscape-micro-station/STEPS/Spindle_Slip_Ring.STEP
Normal file
File diff suppressed because it is too large
Load Diff
199006
A4-simscape-micro-station/STEPS/Spindle_Stator.STEP
Normal file
199006
A4-simscape-micro-station/STEPS/Spindle_Stator.STEP
Normal file
File diff suppressed because one or more lines are too long
12306
A4-simscape-micro-station/STEPS/Tilt_Guide.STEP
Normal file
12306
A4-simscape-micro-station/STEPS/Tilt_Guide.STEP
Normal file
File diff suppressed because it is too large
Load Diff
39764
A4-simscape-micro-station/STEPS/Tilt_Motor.STEP
Normal file
39764
A4-simscape-micro-station/STEPS/Tilt_Motor.STEP
Normal file
File diff suppressed because it is too large
Load Diff
16167
A4-simscape-micro-station/STEPS/Tilt_Motor_Axis.STEP
Normal file
16167
A4-simscape-micro-station/STEPS/Tilt_Motor_Axis.STEP
Normal file
File diff suppressed because it is too large
Load Diff
79980
A4-simscape-micro-station/STEPS/Tilt_Stage.STEP
Normal file
79980
A4-simscape-micro-station/STEPS/Tilt_Stage.STEP
Normal file
File diff suppressed because it is too large
Load Diff
11982
A4-simscape-micro-station/STEPS/Ty_Granite_Frame.STEP
Normal file
11982
A4-simscape-micro-station/STEPS/Ty_Granite_Frame.STEP
Normal file
File diff suppressed because it is too large
Load Diff
36208
A4-simscape-micro-station/STEPS/Ty_Guide.STEP
Normal file
36208
A4-simscape-micro-station/STEPS/Ty_Guide.STEP
Normal file
File diff suppressed because it is too large
Load Diff
10326
A4-simscape-micro-station/STEPS/Ty_Guide_11.STEP
Normal file
10326
A4-simscape-micro-station/STEPS/Ty_Guide_11.STEP
Normal file
File diff suppressed because it is too large
Load Diff
17057
A4-simscape-micro-station/STEPS/Ty_Guide_12.STEP
Normal file
17057
A4-simscape-micro-station/STEPS/Ty_Guide_12.STEP
Normal file
File diff suppressed because it is too large
Load Diff
10326
A4-simscape-micro-station/STEPS/Ty_Guide_21.STEP
Normal file
10326
A4-simscape-micro-station/STEPS/Ty_Guide_21.STEP
Normal file
File diff suppressed because it is too large
Load Diff
17057
A4-simscape-micro-station/STEPS/Ty_Guide_22.STEP
Normal file
17057
A4-simscape-micro-station/STEPS/Ty_Guide_22.STEP
Normal file
File diff suppressed because it is too large
Load Diff
30385
A4-simscape-micro-station/STEPS/Ty_Motor_Rotor.STEP
Normal file
30385
A4-simscape-micro-station/STEPS/Ty_Motor_Rotor.STEP
Normal file
File diff suppressed because it is too large
Load Diff
31879
A4-simscape-micro-station/STEPS/Ty_Motor_Stator.STEP
Normal file
31879
A4-simscape-micro-station/STEPS/Ty_Motor_Stator.STEP
Normal file
File diff suppressed because it is too large
Load Diff
77974
A4-simscape-micro-station/STEPS/Ty_Stage.STEP
Normal file
77974
A4-simscape-micro-station/STEPS/Ty_Stage.STEP
Normal file
File diff suppressed because it is too large
Load Diff
23602
A4-simscape-micro-station/STEPS/granite.STEP
Normal file
23602
A4-simscape-micro-station/STEPS/granite.STEP
Normal file
File diff suppressed because it is too large
Load Diff
BIN
A4-simscape-micro-station/mat/nass_model_conf_log.mat
(Stored with Git LFS)
Normal file
BIN
A4-simscape-micro-station/mat/nass_model_conf_log.mat
(Stored with Git LFS)
Normal file
Binary file not shown.
BIN
A4-simscape-micro-station/mat/nass_model_conf_simscape.mat
(Stored with Git LFS)
Normal file
BIN
A4-simscape-micro-station/mat/nass_model_conf_simscape.mat
(Stored with Git LFS)
Normal file
Binary file not shown.
BIN
A4-simscape-micro-station/mat/nass_model_conf_simulink.mat
(Stored with Git LFS)
Normal file
BIN
A4-simscape-micro-station/mat/nass_model_conf_simulink.mat
(Stored with Git LFS)
Normal file
Binary file not shown.
BIN
A4-simscape-micro-station/mat/nass_model_disturbances.mat
(Stored with Git LFS)
Normal file
BIN
A4-simscape-micro-station/mat/nass_model_disturbances.mat
(Stored with Git LFS)
Normal file
Binary file not shown.
BIN
A4-simscape-micro-station/mat/nass_model_references.mat
(Stored with Git LFS)
Normal file
BIN
A4-simscape-micro-station/mat/nass_model_references.mat
(Stored with Git LFS)
Normal file
Binary file not shown.
BIN
A4-simscape-micro-station/mat/nass_model_stages.mat
(Stored with Git LFS)
Normal file
BIN
A4-simscape-micro-station/mat/nass_model_stages.mat
(Stored with Git LFS)
Normal file
Binary file not shown.
BIN
A4-simscape-micro-station/mat/ustation_compliance_hammer_1.mat
(Stored with Git LFS)
Normal file
BIN
A4-simscape-micro-station/mat/ustation_compliance_hammer_1.mat
(Stored with Git LFS)
Normal file
Binary file not shown.
BIN
A4-simscape-micro-station/mat/ustation_compliance_hammer_10.mat
(Stored with Git LFS)
Normal file
BIN
A4-simscape-micro-station/mat/ustation_compliance_hammer_10.mat
(Stored with Git LFS)
Normal file
Binary file not shown.
BIN
A4-simscape-micro-station/mat/ustation_compliance_hammer_2.mat
(Stored with Git LFS)
Normal file
BIN
A4-simscape-micro-station/mat/ustation_compliance_hammer_2.mat
(Stored with Git LFS)
Normal file
Binary file not shown.
BIN
A4-simscape-micro-station/mat/ustation_compliance_hammer_3.mat
(Stored with Git LFS)
Normal file
BIN
A4-simscape-micro-station/mat/ustation_compliance_hammer_3.mat
(Stored with Git LFS)
Normal file
Binary file not shown.
BIN
A4-simscape-micro-station/mat/ustation_compliance_hammer_4.mat
(Stored with Git LFS)
Normal file
BIN
A4-simscape-micro-station/mat/ustation_compliance_hammer_4.mat
(Stored with Git LFS)
Normal file
Binary file not shown.
BIN
A4-simscape-micro-station/mat/ustation_compliance_hammer_5.mat
(Stored with Git LFS)
Normal file
BIN
A4-simscape-micro-station/mat/ustation_compliance_hammer_5.mat
(Stored with Git LFS)
Normal file
Binary file not shown.
BIN
A4-simscape-micro-station/mat/ustation_compliance_hammer_6.mat
(Stored with Git LFS)
Normal file
BIN
A4-simscape-micro-station/mat/ustation_compliance_hammer_6.mat
(Stored with Git LFS)
Normal file
Binary file not shown.
BIN
A4-simscape-micro-station/mat/ustation_compliance_hammer_7.mat
(Stored with Git LFS)
Normal file
BIN
A4-simscape-micro-station/mat/ustation_compliance_hammer_7.mat
(Stored with Git LFS)
Normal file
Binary file not shown.
BIN
A4-simscape-micro-station/mat/ustation_compliance_hammer_8.mat
(Stored with Git LFS)
Normal file
BIN
A4-simscape-micro-station/mat/ustation_compliance_hammer_8.mat
(Stored with Git LFS)
Normal file
Binary file not shown.
BIN
A4-simscape-micro-station/mat/ustation_compliance_hammer_9.mat
(Stored with Git LFS)
Normal file
BIN
A4-simscape-micro-station/mat/ustation_compliance_hammer_9.mat
(Stored with Git LFS)
Normal file
Binary file not shown.
BIN
A4-simscape-micro-station/mat/ustation_disturbance_psd.mat
(Stored with Git LFS)
Normal file
BIN
A4-simscape-micro-station/mat/ustation_disturbance_psd.mat
(Stored with Git LFS)
Normal file
Binary file not shown.
BIN
A4-simscape-micro-station/mat/ustation_disturbance_sensitivity.mat
(Stored with Git LFS)
Normal file
BIN
A4-simscape-micro-station/mat/ustation_disturbance_sensitivity.mat
(Stored with Git LFS)
Normal file
Binary file not shown.
BIN
A4-simscape-micro-station/mat/ustation_errors_spindle.mat
(Stored with Git LFS)
Normal file
BIN
A4-simscape-micro-station/mat/ustation_errors_spindle.mat
(Stored with Git LFS)
Normal file
Binary file not shown.
BIN
A4-simscape-micro-station/mat/ustation_errors_ty.mat
(Stored with Git LFS)
Normal file
BIN
A4-simscape-micro-station/mat/ustation_errors_ty.mat
(Stored with Git LFS)
Normal file
Binary file not shown.
BIN
A4-simscape-micro-station/mat/ustation_frf_com.mat
(Stored with Git LFS)
Normal file
BIN
A4-simscape-micro-station/mat/ustation_frf_com.mat
(Stored with Git LFS)
Normal file
Binary file not shown.
BIN
A4-simscape-micro-station/mat/ustation_frf_matrix.mat
(Stored with Git LFS)
Normal file
BIN
A4-simscape-micro-station/mat/ustation_frf_matrix.mat
(Stored with Git LFS)
Normal file
Binary file not shown.
BIN
A4-simscape-micro-station/mat/ustation_ground_motion.mat
(Stored with Git LFS)
Normal file
BIN
A4-simscape-micro-station/mat/ustation_ground_motion.mat
(Stored with Git LFS)
Normal file
Binary file not shown.
21
A4-simscape-micro-station/src/circlefit.m
Normal file
21
A4-simscape-micro-station/src/circlefit.m
Normal file
@ -0,0 +1,21 @@
|
||||
function [xc,yc,R,a] = circlefit(x,y)
|
||||
%
|
||||
% [xc yx R] = circfit(x,y)
|
||||
%
|
||||
% fits a circle in x,y plane in a more accurate
|
||||
% (less prone to ill condition )
|
||||
% procedure than circfit2 but using more memory
|
||||
% x,y are column vector where (x(i),y(i)) is a measured point
|
||||
%
|
||||
% result is center point (yc,xc) and radius R
|
||||
% an optional output is the vector of coeficient a
|
||||
% describing the circle's equation
|
||||
%
|
||||
% x^2+y^2+a(1)*x+a(2)*y+a(3)=0
|
||||
%
|
||||
% By: Izhak bucher 25/oct /1991,
|
||||
x=x(:); y=y(:);
|
||||
a=[x y ones(size(x))]\[-(x.^2+y.^2)];
|
||||
xc = -.5*a(1);
|
||||
yc = -.5*a(2);
|
||||
R = sqrt((a(1)^2+a(2)^2)/4-a(3));
|
||||
35
A4-simscape-micro-station/src/computeJacobian.m
Normal file
35
A4-simscape-micro-station/src/computeJacobian.m
Normal file
@ -0,0 +1,35 @@
|
||||
function [stewart] = computeJacobian(stewart)
|
||||
% computeJacobian -
|
||||
%
|
||||
% Syntax: [stewart] = computeJacobian(stewart)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - With at least the following fields:
|
||||
% - geometry.As [3x6] - The 6 unit vectors for each strut expressed in {A}
|
||||
% - geometry.Ab [3x6] - The 6 position of the joints bi expressed in {A}
|
||||
% - actuators.K [6x1] - Total stiffness of the actuators
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - With the 3 added field:
|
||||
% - kinematics.J [6x6] - The Jacobian Matrix
|
||||
% - kinematics.K [6x6] - The Stiffness Matrix
|
||||
% - kinematics.C [6x6] - The Compliance Matrix
|
||||
|
||||
assert(isfield(stewart.geometry, 'As'), 'stewart.geometry should have attribute As')
|
||||
As = stewart.geometry.As;
|
||||
|
||||
assert(isfield(stewart.geometry, 'Ab'), 'stewart.geometry should have attribute Ab')
|
||||
Ab = stewart.geometry.Ab;
|
||||
|
||||
assert(isfield(stewart.actuators, 'K'), 'stewart.actuators should have attribute K')
|
||||
Ki = stewart.actuators.K;
|
||||
|
||||
J = [As' , cross(Ab, As)'];
|
||||
|
||||
K = J'*diag(Ki)*J;
|
||||
|
||||
C = inv(K);
|
||||
|
||||
stewart.kinematics.J = J;
|
||||
stewart.kinematics.K = K;
|
||||
stewart.kinematics.C = C;
|
||||
78
A4-simscape-micro-station/src/computeJointsPose.m
Normal file
78
A4-simscape-micro-station/src/computeJointsPose.m
Normal file
@ -0,0 +1,78 @@
|
||||
function [stewart] = computeJointsPose(stewart)
|
||||
% computeJointsPose -
|
||||
%
|
||||
% Syntax: [stewart] = computeJointsPose(stewart)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - A structure with the following fields
|
||||
% - platform_F.Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
|
||||
% - platform_M.Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
|
||||
% - platform_F.FO_A [3x1] - Position of {A} with respect to {F}
|
||||
% - platform_M.MO_B [3x1] - Position of {B} with respect to {M}
|
||||
% - geometry.FO_M [3x1] - Position of {M} with respect to {F}
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - A structure with the following added fields
|
||||
% - geometry.Aa [3x6] - The i'th column is the position of ai with respect to {A}
|
||||
% - geometry.Ab [3x6] - The i'th column is the position of bi with respect to {A}
|
||||
% - geometry.Ba [3x6] - The i'th column is the position of ai with respect to {B}
|
||||
% - geometry.Bb [3x6] - The i'th column is the position of bi with respect to {B}
|
||||
% - geometry.l [6x1] - The i'th element is the initial length of strut i
|
||||
% - geometry.As [3x6] - The i'th column is the unit vector of strut i expressed in {A}
|
||||
% - geometry.Bs [3x6] - The i'th column is the unit vector of strut i expressed in {B}
|
||||
% - struts_F.l [6x1] - Length of the Fixed part of the i'th strut
|
||||
% - struts_M.l [6x1] - Length of the Mobile part of the i'th strut
|
||||
% - platform_F.FRa [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the bottom of the i'th strut from {F}
|
||||
% - platform_M.MRb [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the top of the i'th strut from {M}
|
||||
|
||||
assert(isfield(stewart.platform_F, 'Fa'), 'stewart.platform_F should have attribute Fa')
|
||||
Fa = stewart.platform_F.Fa;
|
||||
|
||||
assert(isfield(stewart.platform_M, 'Mb'), 'stewart.platform_M should have attribute Mb')
|
||||
Mb = stewart.platform_M.Mb;
|
||||
|
||||
assert(isfield(stewart.platform_F, 'FO_A'), 'stewart.platform_F should have attribute FO_A')
|
||||
FO_A = stewart.platform_F.FO_A;
|
||||
|
||||
assert(isfield(stewart.platform_M, 'MO_B'), 'stewart.platform_M should have attribute MO_B')
|
||||
MO_B = stewart.platform_M.MO_B;
|
||||
|
||||
assert(isfield(stewart.geometry, 'FO_M'), 'stewart.geometry should have attribute FO_M')
|
||||
FO_M = stewart.geometry.FO_M;
|
||||
|
||||
Aa = Fa - repmat(FO_A, [1, 6]);
|
||||
Bb = Mb - repmat(MO_B, [1, 6]);
|
||||
|
||||
Ab = Bb - repmat(-MO_B-FO_M+FO_A, [1, 6]);
|
||||
Ba = Aa - repmat( MO_B+FO_M-FO_A, [1, 6]);
|
||||
|
||||
As = (Ab - Aa)./vecnorm(Ab - Aa); % As_i is the i'th vector of As
|
||||
|
||||
l = vecnorm(Ab - Aa)';
|
||||
|
||||
Bs = (Bb - Ba)./vecnorm(Bb - Ba);
|
||||
|
||||
FRa = zeros(3,3,6);
|
||||
MRb = zeros(3,3,6);
|
||||
|
||||
for i = 1:6
|
||||
FRa(:,:,i) = [cross([0;1;0], As(:,i)) , cross(As(:,i), cross([0;1;0], As(:,i))) , As(:,i)];
|
||||
FRa(:,:,i) = FRa(:,:,i)./vecnorm(FRa(:,:,i));
|
||||
|
||||
MRb(:,:,i) = [cross([0;1;0], Bs(:,i)) , cross(Bs(:,i), cross([0;1;0], Bs(:,i))) , Bs(:,i)];
|
||||
MRb(:,:,i) = MRb(:,:,i)./vecnorm(MRb(:,:,i));
|
||||
end
|
||||
|
||||
stewart.geometry.Aa = Aa;
|
||||
stewart.geometry.Ab = Ab;
|
||||
stewart.geometry.Ba = Ba;
|
||||
stewart.geometry.Bb = Bb;
|
||||
stewart.geometry.As = As;
|
||||
stewart.geometry.Bs = Bs;
|
||||
stewart.geometry.l = l;
|
||||
|
||||
stewart.struts_F.l = l/2;
|
||||
stewart.struts_M.l = l/2;
|
||||
|
||||
stewart.platform_F.FRa = FRa;
|
||||
stewart.platform_M.MRb = MRb;
|
||||
77
A4-simscape-micro-station/src/computeReferencePose.m
Normal file
77
A4-simscape-micro-station/src/computeReferencePose.m
Normal file
@ -0,0 +1,77 @@
|
||||
function [WTr] = computeReferencePose(Dy, Ry, Rz, Dh, Dn)
|
||||
% computeReferencePose - Compute the homogeneous transformation matrix corresponding to the wanted pose of the sample
|
||||
%
|
||||
% Syntax: [WTr] = computeReferencePose(Dy, Ry, Rz, Dh, Dn)
|
||||
%
|
||||
% Inputs:
|
||||
% - Dy - Reference of the Translation Stage [m]
|
||||
% - Ry - Reference of the Tilt Stage [rad]
|
||||
% - Rz - Reference of the Spindle [rad]
|
||||
% - Dh - Reference of the Micro Hexapod (Pitch, Roll, Yaw angles) [m, m, m, rad, rad, rad]
|
||||
% - Dn - Reference of the Nano Hexapod [m, m, m, rad, rad, rad]
|
||||
%
|
||||
% Outputs:
|
||||
% - WTr -
|
||||
|
||||
%% Translation Stage
|
||||
Rty = [1 0 0 0;
|
||||
0 1 0 Dy;
|
||||
0 0 1 0;
|
||||
0 0 0 1];
|
||||
|
||||
%% Tilt Stage - Pure rotating aligned with Ob
|
||||
Rry = [ cos(Ry) 0 sin(Ry) 0;
|
||||
0 1 0 0;
|
||||
-sin(Ry) 0 cos(Ry) 0;
|
||||
0 0 0 1];
|
||||
|
||||
%% Spindle - Rotation along the Z axis
|
||||
Rrz = [cos(Rz) -sin(Rz) 0 0 ;
|
||||
sin(Rz) cos(Rz) 0 0 ;
|
||||
0 0 1 0 ;
|
||||
0 0 0 1 ];
|
||||
|
||||
|
||||
%% Micro-Hexapod
|
||||
Rhx = [1 0 0;
|
||||
0 cos(Dh(4)) -sin(Dh(4));
|
||||
0 sin(Dh(4)) cos(Dh(4))];
|
||||
|
||||
Rhy = [ cos(Dh(5)) 0 sin(Dh(5));
|
||||
0 1 0;
|
||||
-sin(Dh(5)) 0 cos(Dh(5))];
|
||||
|
||||
Rhz = [cos(Dh(6)) -sin(Dh(6)) 0;
|
||||
sin(Dh(6)) cos(Dh(6)) 0;
|
||||
0 0 1];
|
||||
|
||||
Rh = [1 0 0 Dh(1) ;
|
||||
0 1 0 Dh(2) ;
|
||||
0 0 1 Dh(3) ;
|
||||
0 0 0 1 ];
|
||||
|
||||
Rh(1:3, 1:3) = Rhz*Rhy*Rhx;
|
||||
|
||||
%% Nano-Hexapod
|
||||
Rnx = [1 0 0;
|
||||
0 cos(Dn(4)) -sin(Dn(4));
|
||||
0 sin(Dn(4)) cos(Dn(4))];
|
||||
|
||||
Rny = [ cos(Dn(5)) 0 sin(Dn(5));
|
||||
0 1 0;
|
||||
-sin(Dn(5)) 0 cos(Dn(5))];
|
||||
|
||||
Rnz = [cos(Dn(6)) -sin(Dn(6)) 0;
|
||||
sin(Dn(6)) cos(Dn(6)) 0;
|
||||
0 0 1];
|
||||
|
||||
Rn = [1 0 0 Dn(1) ;
|
||||
0 1 0 Dn(2) ;
|
||||
0 0 1 Dn(3) ;
|
||||
0 0 0 1 ];
|
||||
|
||||
Rn(1:3, 1:3) = Rnz*Rny*Rnx;
|
||||
|
||||
%% Total Homogeneous transformation
|
||||
WTr = Rty*Rry*Rrz*Rh*Rn;
|
||||
end
|
||||
141
A4-simscape-micro-station/src/describeMicroStationSetup.m
Normal file
141
A4-simscape-micro-station/src/describeMicroStationSetup.m
Normal file
@ -0,0 +1,141 @@
|
||||
function [] = describeMicroStationSetup()
|
||||
% describeMicroStationSetup -
|
||||
%
|
||||
% Syntax: [] = describeMicroStationSetup()
|
||||
%
|
||||
% Inputs:
|
||||
% - -
|
||||
%
|
||||
% Outputs:
|
||||
% - -
|
||||
|
||||
load('./mat/nass_model_conf_simscape.mat', 'conf_simscape');
|
||||
|
||||
fprintf('Simscape Configuration:\n');
|
||||
|
||||
if conf_simscape.type == 1
|
||||
fprintf('- Gravity is included\n');
|
||||
else
|
||||
fprintf('- Gravity is not included\n');
|
||||
end
|
||||
|
||||
fprintf('\n');
|
||||
|
||||
load('./mat/nass_model_disturbances.mat', 'args');
|
||||
|
||||
fprintf('Disturbances:\n');
|
||||
if ~args.enable
|
||||
fprintf('- No disturbance is included\n');
|
||||
else
|
||||
if args.Dwx && args.Dwy && args.Dwz
|
||||
fprintf('- Ground motion\n');
|
||||
end
|
||||
if args.Fdy_x && args.Fdy_z
|
||||
fprintf('- Vibrations of the Translation Stage\n');
|
||||
end
|
||||
if args.Frz_z
|
||||
fprintf('- Vibrations of the Spindle\n');
|
||||
end
|
||||
end
|
||||
fprintf('\n');
|
||||
|
||||
load('./mat/nass_model_references.mat', 'args');
|
||||
|
||||
fprintf('Reference Tracking:\n');
|
||||
fprintf('- Translation Stage:\n');
|
||||
switch args.Dy_type
|
||||
case 'constant'
|
||||
fprintf(' - Constant Position\n');
|
||||
fprintf(' - Dy = %.0f [mm]\n', args.Dy_amplitude*1e3);
|
||||
case 'triangular'
|
||||
fprintf(' - Triangular Path\n');
|
||||
fprintf(' - Amplitude = %.0f [mm]\n', args.Dy_amplitude*1e3);
|
||||
fprintf(' - Period = %.0f [s]\n', args.Dy_period);
|
||||
case 'sinusoidal'
|
||||
fprintf(' - Sinusoidal Path\n');
|
||||
fprintf(' - Amplitude = %.0f [mm]\n', args.Dy_amplitude*1e3);
|
||||
fprintf(' - Period = %.0f [s]\n', args.Dy_period);
|
||||
end
|
||||
|
||||
fprintf('- Tilt Stage:\n');
|
||||
switch args.Ry_type
|
||||
case 'constant'
|
||||
fprintf(' - Constant Position\n');
|
||||
fprintf(' - Ry = %.0f [mm]\n', args.Ry_amplitude*1e3);
|
||||
case 'triangular'
|
||||
fprintf(' - Triangular Path\n');
|
||||
fprintf(' - Amplitude = %.0f [mm]\n', args.Ry_amplitude*1e3);
|
||||
fprintf(' - Period = %.0f [s]\n', args.Ry_period);
|
||||
case 'sinusoidal'
|
||||
fprintf(' - Sinusoidal Path\n');
|
||||
fprintf(' - Amplitude = %.0f [mm]\n', args.Ry_amplitude*1e3);
|
||||
fprintf(' - Period = %.0f [s]\n', args.Ry_period);
|
||||
end
|
||||
|
||||
fprintf('- Spindle:\n');
|
||||
switch args.Rz_type
|
||||
case 'constant'
|
||||
fprintf(' - Constant Position\n');
|
||||
fprintf(' - Rz = %.0f [deg]\n', 180/pi*args.Rz_amplitude);
|
||||
case { 'rotating', 'rotating-not-filtered' }
|
||||
fprintf(' - Rotating\n');
|
||||
fprintf(' - Speed = %.0f [rpm]\n', 60/args.Rz_period);
|
||||
end
|
||||
|
||||
|
||||
fprintf('- Micro Hexapod:\n');
|
||||
switch args.Dh_type
|
||||
case 'constant'
|
||||
fprintf(' - Constant Position\n');
|
||||
fprintf(' - Dh = %.0f, %.0f, %.0f [mm]\n', args.Dh_pos(1), args.Dh_pos(2), args.Dh_pos(3));
|
||||
fprintf(' - Rh = %.0f, %.0f, %.0f [deg]\n', args.Dh_pos(4), args.Dh_pos(5), args.Dh_pos(6));
|
||||
end
|
||||
|
||||
fprintf('\n');
|
||||
|
||||
load('./mat/nass_model_stages.mat', 'ground', 'granite', 'ty', 'ry', 'rz', 'micro_hexapod', 'axisc');
|
||||
|
||||
fprintf('Micro Station:\n');
|
||||
|
||||
if granite.type == 1 && ...
|
||||
ty.type == 1 && ...
|
||||
ry.type == 1 && ...
|
||||
rz.type == 1 && ...
|
||||
micro_hexapod.type == 1;
|
||||
fprintf('- All stages are rigid\n');
|
||||
elseif granite.type == 2 && ...
|
||||
ty.type == 2 && ...
|
||||
ry.type == 2 && ...
|
||||
rz.type == 2 && ...
|
||||
micro_hexapod.type == 2;
|
||||
fprintf('- All stages are flexible\n');
|
||||
else
|
||||
if granite.type == 1 || granite.type == 4
|
||||
fprintf('- Granite is rigid\n');
|
||||
else
|
||||
fprintf('- Granite is flexible\n');
|
||||
end
|
||||
if ty.type == 1 || ty.type == 4
|
||||
fprintf('- Translation Stage is rigid\n');
|
||||
else
|
||||
fprintf('- Translation Stage is flexible\n');
|
||||
end
|
||||
if ry.type == 1 || ry.type == 4
|
||||
fprintf('- Tilt Stage is rigid\n');
|
||||
else
|
||||
fprintf('- Tilt Stage is flexible\n');
|
||||
end
|
||||
if rz.type == 1 || rz.type == 4
|
||||
fprintf('- Spindle is rigid\n');
|
||||
else
|
||||
fprintf('- Spindle is flexible\n');
|
||||
end
|
||||
if micro_hexapod.type == 1 || micro_hexapod.type == 4
|
||||
fprintf('- Micro Hexapod is rigid\n');
|
||||
else
|
||||
fprintf('- Micro Hexapod is flexible\n');
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
fprintf('\n');
|
||||
39
A4-simscape-micro-station/src/generateGeneralConfiguration.m
Normal file
39
A4-simscape-micro-station/src/generateGeneralConfiguration.m
Normal file
@ -0,0 +1,39 @@
|
||||
function [stewart] = generateGeneralConfiguration(stewart, args)
|
||||
% generateGeneralConfiguration - Generate a Very General Configuration
|
||||
%
|
||||
% Syntax: [stewart] = generateGeneralConfiguration(stewart, args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Can have the following fields:
|
||||
% - FH [1x1] - Height of the position of the fixed joints with respect to the frame {F} [m]
|
||||
% - FR [1x1] - Radius of the position of the fixed joints in the X-Y [m]
|
||||
% - FTh [6x1] - Angles of the fixed joints in the X-Y plane with respect to the X axis [rad]
|
||||
% - MH [1x1] - Height of the position of the mobile joints with respect to the frame {M} [m]
|
||||
% - FR [1x1] - Radius of the position of the mobile joints in the X-Y [m]
|
||||
% - MTh [6x1] - Angles of the mobile joints in the X-Y plane with respect to the X axis [rad]
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - updated Stewart structure with the added fields:
|
||||
% - platform_F.Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
|
||||
% - platform_M.Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
|
||||
|
||||
arguments
|
||||
stewart
|
||||
args.FH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
|
||||
args.FR (1,1) double {mustBeNumeric, mustBePositive} = 115e-3;
|
||||
args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180);
|
||||
args.MH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
|
||||
args.MR (1,1) double {mustBeNumeric, mustBePositive} = 90e-3;
|
||||
args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180);
|
||||
end
|
||||
|
||||
Fa = zeros(3,6);
|
||||
Mb = zeros(3,6);
|
||||
|
||||
for i = 1:6
|
||||
Fa(:,i) = [args.FR*cos(args.FTh(i)); args.FR*sin(args.FTh(i)); args.FH];
|
||||
Mb(:,i) = [args.MR*cos(args.MTh(i)); args.MR*sin(args.MTh(i)); -args.MH];
|
||||
end
|
||||
|
||||
stewart.platform_F.Fa = Fa;
|
||||
stewart.platform_M.Mb = Mb;
|
||||
@ -0,0 +1,59 @@
|
||||
function [stewart] = initializeCylindricalPlatforms(stewart, args)
|
||||
% initializeCylindricalPlatforms - Initialize the geometry of the Fixed and Mobile Platforms
|
||||
%
|
||||
% Syntax: [stewart] = initializeCylindricalPlatforms(args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Structure with the following fields:
|
||||
% - Fpm [1x1] - Fixed Platform Mass [kg]
|
||||
% - Fph [1x1] - Fixed Platform Height [m]
|
||||
% - Fpr [1x1] - Fixed Platform Radius [m]
|
||||
% - Mpm [1x1] - Mobile Platform Mass [kg]
|
||||
% - Mph [1x1] - Mobile Platform Height [m]
|
||||
% - Mpr [1x1] - Mobile Platform Radius [m]
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - updated Stewart structure with the added fields:
|
||||
% - platform_F [struct] - structure with the following fields:
|
||||
% - type = 1
|
||||
% - M [1x1] - Fixed Platform Mass [kg]
|
||||
% - I [3x3] - Fixed Platform Inertia matrix [kg*m^2]
|
||||
% - H [1x1] - Fixed Platform Height [m]
|
||||
% - R [1x1] - Fixed Platform Radius [m]
|
||||
% - platform_M [struct] - structure with the following fields:
|
||||
% - M [1x1] - Mobile Platform Mass [kg]
|
||||
% - I [3x3] - Mobile Platform Inertia matrix [kg*m^2]
|
||||
% - H [1x1] - Mobile Platform Height [m]
|
||||
% - R [1x1] - Mobile Platform Radius [m]
|
||||
|
||||
arguments
|
||||
stewart
|
||||
args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
||||
args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
|
||||
args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 125e-3
|
||||
args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
||||
args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
|
||||
args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
|
||||
end
|
||||
|
||||
I_F = diag([1/12*args.Fpm * (3*args.Fpr^2 + args.Fph^2), ...
|
||||
1/12*args.Fpm * (3*args.Fpr^2 + args.Fph^2), ...
|
||||
1/2 *args.Fpm * args.Fpr^2]);
|
||||
|
||||
I_M = diag([1/12*args.Mpm * (3*args.Mpr^2 + args.Mph^2), ...
|
||||
1/12*args.Mpm * (3*args.Mpr^2 + args.Mph^2), ...
|
||||
1/2 *args.Mpm * args.Mpr^2]);
|
||||
|
||||
stewart.platform_F.type = 1;
|
||||
|
||||
stewart.platform_F.I = I_F;
|
||||
stewart.platform_F.M = args.Fpm;
|
||||
stewart.platform_F.R = args.Fpr;
|
||||
stewart.platform_F.H = args.Fph;
|
||||
|
||||
stewart.platform_M.type = 1;
|
||||
|
||||
stewart.platform_M.I = I_M;
|
||||
stewart.platform_M.M = args.Mpm;
|
||||
stewart.platform_M.R = args.Mpr;
|
||||
stewart.platform_M.H = args.Mph;
|
||||
71
A4-simscape-micro-station/src/initializeCylindricalStruts.m
Normal file
71
A4-simscape-micro-station/src/initializeCylindricalStruts.m
Normal file
@ -0,0 +1,71 @@
|
||||
function [stewart] = initializeCylindricalStruts(stewart, args)
|
||||
% initializeCylindricalStruts - Define the mass and moment of inertia of cylindrical struts
|
||||
%
|
||||
% Syntax: [stewart] = initializeCylindricalStruts(args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Structure with the following fields:
|
||||
% - Fsm [1x1] - Mass of the Fixed part of the struts [kg]
|
||||
% - Fsh [1x1] - Height of cylinder for the Fixed part of the struts [m]
|
||||
% - Fsr [1x1] - Radius of cylinder for the Fixed part of the struts [m]
|
||||
% - Msm [1x1] - Mass of the Mobile part of the struts [kg]
|
||||
% - Msh [1x1] - Height of cylinder for the Mobile part of the struts [m]
|
||||
% - Msr [1x1] - Radius of cylinder for the Mobile part of the struts [m]
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - updated Stewart structure with the added fields:
|
||||
% - struts_F [struct] - structure with the following fields:
|
||||
% - M [6x1] - Mass of the Fixed part of the struts [kg]
|
||||
% - I [3x3x6] - Moment of Inertia for the Fixed part of the struts [kg*m^2]
|
||||
% - H [6x1] - Height of cylinder for the Fixed part of the struts [m]
|
||||
% - R [6x1] - Radius of cylinder for the Fixed part of the struts [m]
|
||||
% - struts_M [struct] - structure with the following fields:
|
||||
% - M [6x1] - Mass of the Mobile part of the struts [kg]
|
||||
% - I [3x3x6] - Moment of Inertia for the Mobile part of the struts [kg*m^2]
|
||||
% - H [6x1] - Height of cylinder for the Mobile part of the struts [m]
|
||||
% - R [6x1] - Radius of cylinder for the Mobile part of the struts [m]
|
||||
|
||||
arguments
|
||||
stewart
|
||||
args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
|
||||
args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
|
||||
args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
|
||||
args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
|
||||
args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
|
||||
args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
|
||||
end
|
||||
|
||||
Fsm = ones(6,1).*args.Fsm;
|
||||
Fsh = ones(6,1).*args.Fsh;
|
||||
Fsr = ones(6,1).*args.Fsr;
|
||||
|
||||
Msm = ones(6,1).*args.Msm;
|
||||
Msh = ones(6,1).*args.Msh;
|
||||
Msr = ones(6,1).*args.Msr;
|
||||
|
||||
I_F = zeros(3, 3, 6); % Inertia of the "fixed" part of the strut
|
||||
I_M = zeros(3, 3, 6); % Inertia of the "mobile" part of the strut
|
||||
|
||||
for i = 1:6
|
||||
I_F(:,:,i) = diag([1/12 * Fsm(i) * (3*Fsr(i)^2 + Fsh(i)^2), ...
|
||||
1/12 * Fsm(i) * (3*Fsr(i)^2 + Fsh(i)^2), ...
|
||||
1/2 * Fsm(i) * Fsr(i)^2]);
|
||||
|
||||
I_M(:,:,i) = diag([1/12 * Msm(i) * (3*Msr(i)^2 + Msh(i)^2), ...
|
||||
1/12 * Msm(i) * (3*Msr(i)^2 + Msh(i)^2), ...
|
||||
1/2 * Msm(i) * Msr(i)^2]);
|
||||
end
|
||||
|
||||
stewart.struts_M.type = 1;
|
||||
|
||||
stewart.struts_M.I = I_M;
|
||||
stewart.struts_M.M = Msm;
|
||||
stewart.struts_M.R = Msr;
|
||||
stewart.struts_M.H = Msh;
|
||||
|
||||
stewart.struts_F.type = 1;
|
||||
|
||||
stewart.struts_F.I = I_F;
|
||||
stewart.struts_F.M = Fsm;
|
||||
stewart.struts_F.R = Fsr;
|
||||
stewart.struts_F.H = Fsh;
|
||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
x
Reference in New Issue
Block a user