add all files
This commit is contained in:
166
C1-test-bench-apa/test_apa_3_model_2dof.m
Normal file
166
C1-test-bench-apa/test_apa_3_model_2dof.m
Normal file
@@ -0,0 +1,166 @@
|
||||
%% Clear Workspace and Close figures
|
||||
clear; close all; clc;
|
||||
|
||||
%% Intialize Laplace variable
|
||||
s = zpk('s');
|
||||
|
||||
%% Path for functions, data and scripts
|
||||
addpath('./src/'); % Path for scripts
|
||||
addpath('./mat/'); % Path for data
|
||||
|
||||
addpath('./STEPS/'); % Path for Simscape Model
|
||||
|
||||
%% Linearization options
|
||||
opts = linearizeOptions;
|
||||
opts.SampleTime = 0;
|
||||
|
||||
%% Open Simscape Model
|
||||
mdl = 'test_apa_simscape'; % Name of the Simulink File
|
||||
open(mdl); % Open Simscape Model
|
||||
|
||||
%% Colors for the figures
|
||||
colors = colororder;
|
||||
|
||||
%% Input/Output definition of the Model
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/u'], 1, 'openinput'); io_i = io_i + 1; % DAC Voltage
|
||||
io(io_i) = linio([mdl, '/Vs'], 1, 'openoutput'); io_i = io_i + 1; % Sensor Voltage
|
||||
io(io_i) = linio([mdl, '/de'], 1, 'openoutput'); io_i = io_i + 1; % Encoder
|
||||
|
||||
%% Frequency vector for analysis
|
||||
freqs = 5*logspace(0, 3, 1000);
|
||||
|
||||
%% Stiffness values for the 2DoF APA model
|
||||
k1 = 0.38e6; % Estimated Shell Stiffness [N/m]
|
||||
|
||||
w0 = 2*pi*95; % Resonance frequency [rad/s]
|
||||
m = 5.7; % Suspended mass [kg]
|
||||
ktot = m*(w0)^2; % Total Axial Stiffness to have to wanted resonance frequency [N/m]
|
||||
|
||||
ka = 1.5*(ktot-k1); % Stiffness of the (two) actuator stacks [N/m]
|
||||
ke = 2*ka; % Stiffness of the Sensor stack [N/m]
|
||||
|
||||
%% Damping values for the 2DoF APA model
|
||||
c1 = 5; % Damping for the Shell [N/(m/s)]
|
||||
ca = 50; % Damping of the actuators stacks [N/(m/s)]
|
||||
ce = 2*ca; % Damping of the sensor stack [N/(m/s)]
|
||||
|
||||
%% Estimation ot the sensor and actuator sensitivities
|
||||
% Initialize the structure with unitary sensor and actuator "sensitivities"
|
||||
n_hexapod = struct();
|
||||
n_hexapod.actuator = initializeAPA(...
|
||||
'type', '2dof', ...
|
||||
'k', k1, ...
|
||||
'ka', ka, ...
|
||||
'ke', ke, ...
|
||||
'c', c1, ...
|
||||
'ca', ca, ...
|
||||
'ce', ce, ...
|
||||
'Ga', 1, ... % Actuator constant [N/V]
|
||||
'Gs', 1 ... % Sensor constant [V/m]
|
||||
);
|
||||
|
||||
c_granite = 50; % Do not take into account damping added by the air bearing
|
||||
|
||||
% Run the linearization
|
||||
G_norm = linearize(mdl, io, 0.0, opts);
|
||||
G_norm.InputName = {'u'};
|
||||
G_norm.OutputName = {'Vs', 'de'};
|
||||
|
||||
% Load Identification Data to estimate the two sensitivities
|
||||
load('meas_apa_frf.mat', 'f', 'Ts', 'enc_frf', 'iff_frf', 'apa_nums');
|
||||
|
||||
% Estimation ot the Actuator sensitivity
|
||||
fa = 10; % Frequency where the two FRF should match [Hz]
|
||||
[~, i_f] = min(abs(f - fa));
|
||||
ga = -abs(enc_frf(i_f,1))./abs(evalfr(G_norm('de', 'u'), 1i*2*pi*fa));
|
||||
|
||||
% Estimation ot the Sensor sensitivity
|
||||
fs = 600; % Frequency where the two FRF should match [Hz]
|
||||
[~, i_f] = min(abs(f - fs));
|
||||
gs = -abs(iff_frf(i_f,1))./abs(evalfr(G_norm('Vs', 'u'), 1i*2*pi*fs))/ga;
|
||||
|
||||
%% 2DoF APA300ML with optimized parameters
|
||||
n_hexapod = struct();
|
||||
n_hexapod.actuator = initializeAPA( ...
|
||||
'type', '2dof', ...
|
||||
'k', k1, ...
|
||||
'ka', ka, ...
|
||||
'ke', ke, ...
|
||||
'c', c1, ...
|
||||
'ca', ca, ...
|
||||
'ce', ce, ...
|
||||
'Ga', ga, ...
|
||||
'Gs', gs ...
|
||||
);
|
||||
|
||||
%% Identification of the APA300ML with optimized parameters
|
||||
G_2dof = exp(-s*Ts)*linearize(mdl, io, 0.0, opts);
|
||||
G_2dof.InputName = {'u'};
|
||||
G_2dof.OutputName = {'Vs', 'de'};
|
||||
|
||||
%% Comparison of the measured FRF and the optimized 2DoF model of the APA300ML
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(f, abs(enc_frf(:, 1)), 'color', [0,0,0,0.2], 'DisplayName', 'Identified');
|
||||
for i = 1:length(apa_nums)
|
||||
plot(f, abs(enc_frf(:, i)), 'color', [0,0,0,0.2], 'HandleVisibility', 'off');
|
||||
end
|
||||
plot(freqs, abs(squeeze(freqresp(G_2dof('de', 'u'), freqs, 'Hz'))), '--', 'color', colors(2,:), 'DisplayName', '2DoF Model')
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude $d_e/u$ [m/V]'); set(gca, 'XTickLabel',[]);
|
||||
hold off;
|
||||
ylim([1e-8, 1e-3]);
|
||||
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
for i = 1:length(apa_nums)
|
||||
plot(f, 180/pi*angle(enc_frf(:, i)), 'color', [0,0,0,0.2]);
|
||||
end
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_2dof('de', 'u'), 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, 180]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([10, 2e3]);
|
||||
|
||||
%% Comparison of the measured FRF and the optimized 2DoF model of the APA300ML
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(f, abs(iff_frf(:, 1)), 'color', [0,0,0,0.2], 'DisplayName', 'Identified');
|
||||
for i = 2:length(apa_nums)
|
||||
plot(f, abs(iff_frf(:, i)), 'color', [0,0,0,0.2], 'HandleVisibility', 'off');
|
||||
end
|
||||
plot(freqs, abs(squeeze(freqresp(G_2dof('Vs', 'u'), freqs, 'Hz'))), '--', 'color', colors(2,:), 'DisplayName', '2DoF Model')
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude $V_s/u$ [V/V]'); set(gca, 'XTickLabel',[]);
|
||||
hold off;
|
||||
ylim([1e-2, 1e2]);
|
||||
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
for i = 1:length(apa_nums)
|
||||
plot(f, 180/pi*angle(iff_frf(:, i)), 'color', [0,0,0,0.2]);
|
||||
end
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_2dof('Vs', 'u'), 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, 180]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([10, 2e3]);
|
||||
Reference in New Issue
Block a user