add all files
This commit is contained in:
@@ -0,0 +1 @@
|
||||
*.mat filter=lfs diff=lfs merge=lfs -text
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -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]);
|
||||
@@ -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');
|
||||
@@ -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');
|
||||
@@ -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])
|
||||
@@ -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])
|
||||
@@ -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])
|
||||
@@ -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]);
|
||||
@@ -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])
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -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)]);
|
||||
@@ -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;
|
||||
@@ -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$'})
|
||||
@@ -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)]);
|
||||
@@ -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)]);
|
||||
@@ -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)]);
|
||||
@@ -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)]);
|
||||
@@ -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])
|
||||
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
|
||||
@@ -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.';
|
||||
@@ -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
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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
|
||||
@@ -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]);
|
||||
@@ -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
|
||||
@@ -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]);
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because one or more lines are too long
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -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));
|
||||
@@ -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;
|
||||
@@ -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;
|
||||
@@ -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
|
||||
@@ -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');
|
||||
@@ -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;
|
||||
@@ -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
Reference in New Issue
Block a user