Update figures and tangle matlab code
This commit is contained in:
95
matlab/force_sensor_identification.m
Normal file
95
matlab/force_sensor_identification.m
Normal file
@@ -0,0 +1,95 @@
|
||||
%% Clear Workspace and Close figures
|
||||
clear; close all; clc;
|
||||
|
||||
%% Intialize Laplace variable
|
||||
s = zpk('s');
|
||||
|
||||
addpath('./mat/');
|
||||
|
||||
% Load Data :ignore:
|
||||
% The data are loaded:
|
||||
|
||||
load('apa95ml_5kg_2a_1s.mat', 't', 'u', 'v');
|
||||
|
||||
|
||||
|
||||
% Any offset is removed.
|
||||
|
||||
u = detrend(u, 0); % Speedgoat DAC output Voltage [V]
|
||||
v = detrend(v, 0); % Speedgoat ADC input Voltage (sensor stack) [V]
|
||||
|
||||
|
||||
|
||||
% Here, the amplifier gain is 20.
|
||||
|
||||
u = 20*u; % Actuator Stack Voltage [V]
|
||||
|
||||
% Compute TF estimate and Coherence :ignore:
|
||||
% The transfer function from the actuator voltage $V_a$ to the force sensor stack voltage $V_s$ is computed.
|
||||
|
||||
|
||||
Ts = t(end)/(length(t)-1);
|
||||
Fs = 1/Ts;
|
||||
|
||||
win = hann(ceil(5/Ts));
|
||||
|
||||
[tf_est, f] = tfestimate(u, v, win, [], [], 1/Ts);
|
||||
[coh, ~] = mscohere( u, v, win, [], [], 1/Ts);
|
||||
|
||||
|
||||
|
||||
% The coherence is shown in Figure [[fig:apa95ml_5kg_cedrat_coh]].
|
||||
|
||||
|
||||
figure;
|
||||
|
||||
hold on;
|
||||
plot(f, coh, 'k-')
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
|
||||
ylabel('Coherence'); xlabel('Frequency [Hz]');
|
||||
hold off;
|
||||
xlim([10, 5e3]);
|
||||
|
||||
|
||||
|
||||
% #+name: fig:apa95ml_5kg_cedrat_coh
|
||||
% #+caption: Coherence
|
||||
% #+RESULTS:
|
||||
% [[file:figs/apa95ml_5kg_cedrat_coh.png]]
|
||||
|
||||
% The Simscape model is loaded and compared with the identified dynamics in Figure [[fig:bode_plot_force_sensor_voltage_comp_fem]].
|
||||
% The non-minimum phase zero is just a side effect of the not so great identification.
|
||||
% Taking longer measurements would results in a minimum phase zero.
|
||||
|
||||
|
||||
load('mat/fem_simscape_models.mat', 'Gfm');
|
||||
|
||||
freqs = logspace(1, 4, 1000);
|
||||
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(f, abs(tf_est))
|
||||
plot(freqs, abs(squeeze(freqresp(Gfm, freqs, 'Hz'))))
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
|
||||
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
|
||||
hold off;
|
||||
ylim([1e-3, 1e1]);
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
plot(f, 180/pi*angle(tf_est), ...
|
||||
'DisplayName', 'Identification')
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Gfm, freqs, 'Hz'))), ...
|
||||
'DisplayName', 'FEM')
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
|
||||
ylabel('Phase'); xlabel('Frequency [Hz]');
|
||||
hold off;
|
||||
ylim([-180, 180]);
|
||||
yticks(-180:90:180);
|
||||
legend('location', 'northeast')
|
||||
|
||||
linkaxes([ax1,ax2], 'x');
|
||||
xlim([10, 5e3]);
|
||||
34
matlab/huddle_test.m
Normal file
34
matlab/huddle_test.m
Normal file
@@ -0,0 +1,34 @@
|
||||
%% Clear Workspace and Close figures
|
||||
clear; close all; clc;
|
||||
|
||||
%% Intialize Laplace variable
|
||||
s = zpk('s');
|
||||
|
||||
addpath('./mat/');
|
||||
|
||||
% Load Data :noexport:
|
||||
|
||||
load('huddle_test.mat', 't', 'y');
|
||||
|
||||
y = y - mean(y(1000:end));
|
||||
|
||||
% Time Domain Data
|
||||
|
||||
figure;
|
||||
plot(t(1000:end), y(1000:end))
|
||||
ylabel('Output Displacement [m]'); xlabel('Time [s]');
|
||||
|
||||
% PSD of Measurement Noise
|
||||
|
||||
Ts = t(end)/(length(t)-1);
|
||||
Fs = 1/Ts;
|
||||
|
||||
win = hanning(ceil(1*Fs));
|
||||
|
||||
[pxx, f] = pwelch(y(1000:end), win, [], [], Fs);
|
||||
|
||||
figure;
|
||||
plot(f, sqrt(pxx));
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('ASD [$m/\sqrt{Hz}$]');
|
||||
xlim([1, Fs/2]);
|
||||
307
matlab/integral_force_feedback.m
Normal file
307
matlab/integral_force_feedback.m
Normal file
@@ -0,0 +1,307 @@
|
||||
%% Clear Workspace and Close figures
|
||||
clear; close all; clc;
|
||||
|
||||
%% Intialize Laplace variable
|
||||
s = zpk('s');
|
||||
|
||||
addpath('./mat/');
|
||||
|
||||
% IFF Plant
|
||||
% From the identified plant, a model of the transfer function from the actuator stack voltage to the force sensor generated voltage is developed.
|
||||
|
||||
|
||||
w_z = 2*pi*111; % Zeros frequency [rad/s]
|
||||
w_p = 2*pi*255; % Pole frequency [rad/s]
|
||||
xi_z = 0.05;
|
||||
xi_p = 0.015;
|
||||
G_inf = 0.1;
|
||||
|
||||
Gi = G_inf*(s^2 + 2*xi_z*w_z*s + w_z^2)/(s^2 + 2*xi_p*w_p*s + w_p^2);
|
||||
|
||||
|
||||
|
||||
% Its bode plot is shown in Figure [[fig:iff_plant_identification_apa95ml]].
|
||||
|
||||
|
||||
freqs = logspace(1, 4, 1000);
|
||||
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(Gi, freqs, 'Hz'))), 'k-')
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
|
||||
ylabel('Amplitude'); set(gca, 'XTickLabel', []);
|
||||
hold off;
|
||||
ylim([1e-3, 1e1]);
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Gi, freqs, 'Hz'))), 'k-')
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
|
||||
ylabel('Phase'); xlabel('Frequency [Hz]');
|
||||
hold off;
|
||||
ylim([-180, 180]);
|
||||
yticks(-180:90:180);
|
||||
|
||||
linkaxes([ax1,ax2], 'x');
|
||||
xlim([10, 5e3]);
|
||||
|
||||
|
||||
|
||||
% #+name: fig:iff_plant_identification_apa95ml
|
||||
% #+caption: Bode plot of the IFF plant
|
||||
% #+RESULTS:
|
||||
% [[file:figs/iff_plant_identification_apa95ml.png]]
|
||||
|
||||
% The controller used in the Integral Force Feedback Architecture is:
|
||||
% \begin{equation}
|
||||
% K_{\text{IFF}}(s) = \frac{g}{s + 2\cdot 2\pi} \cdot \frac{s}{s + 0.5 \cdot 2\pi}
|
||||
% \end{equation}
|
||||
% where $g$ is a gain that can be tuned.
|
||||
|
||||
% Above 2 Hz the controller is basically an integrator, whereas an high pass filter is added at 0.5Hz to further reduce the low frequency gain.
|
||||
|
||||
% In the frequency band of interest, this controller should mostly act as a pure integrator.
|
||||
|
||||
% The Root Locus corresponding to this controller is shown in Figure [[fig:root_locus_iff_apa95ml_identification]].
|
||||
|
||||
|
||||
gains = logspace(0, 5, 1000);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(real(pole(Gi)), imag(pole(Gi)), 'kx');
|
||||
plot(real(tzero(Gi)), imag(tzero(Gi)), 'ko');
|
||||
for i = 1:length(gains)
|
||||
cl_poles = pole(feedback(Gi, (gains(i)/(s + 2*2*pi)*s/(s + 0.5*2*pi))));
|
||||
plot(real(cl_poles), imag(cl_poles), 'k.');
|
||||
end
|
||||
ylim([0, 1800]);
|
||||
xlim([-1600,200]);
|
||||
xlabel('Real Part')
|
||||
ylabel('Imaginary Part')
|
||||
axis square
|
||||
|
||||
% First tests with few gains
|
||||
% The controller is now implemented in practice, and few controller gains are tested: $g = 0$, $g = 10$ and $g = 100$.
|
||||
|
||||
% For each controller gain, the identification shown in Figure [[fig:test_bench_apa_schematic_iff]] is performed.
|
||||
|
||||
iff_g10 = load('apa95ml_iff_g10_res.mat', 'u', 't', 'y', 'v');
|
||||
iff_g100 = load('apa95ml_iff_g100_res.mat', 'u', 't', 'y', 'v');
|
||||
iff_of = load('apa95ml_iff_off_res.mat', 'u', 't', 'y', 'v');
|
||||
|
||||
Ts = 1e-4;
|
||||
win = hann(ceil(10/Ts));
|
||||
|
||||
[tf_iff_g10, f] = tfestimate(iff_g10.u, iff_g10.y, win, [], [], 1/Ts);
|
||||
[co_iff_g10, ~] = mscohere( iff_g10.u, iff_g10.y, win, [], [], 1/Ts);
|
||||
|
||||
[tf_iff_g100, ~] = tfestimate(iff_g100.u, iff_g100.y, win, [], [], 1/Ts);
|
||||
[co_iff_g100, ~] = mscohere( iff_g100.u, iff_g100.y, win, [], [], 1/Ts);
|
||||
|
||||
[tf_iff_of, ~] = tfestimate(iff_of.u, iff_of.y, win, [], [], 1/Ts);
|
||||
[co_iff_of, ~] = mscohere( iff_of.u, iff_of.y, win, [], [], 1/Ts);
|
||||
|
||||
|
||||
|
||||
% The coherence between the excitation signal and the mass displacement as measured by the interferometer is shown in Figure [[fig:iff_first_test_coherence]].
|
||||
|
||||
|
||||
figure;
|
||||
|
||||
hold on;
|
||||
plot(f, co_iff_of, '-', 'DisplayName', 'g=0')
|
||||
plot(f, co_iff_g10, '-', 'DisplayName', 'g=10')
|
||||
plot(f, co_iff_g100, '-', 'DisplayName', 'g=100')
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
|
||||
ylabel('Coherence'); xlabel('Frequency [Hz]');
|
||||
hold off;
|
||||
legend();
|
||||
xlim([60, 600])
|
||||
|
||||
|
||||
|
||||
% #+name: fig:iff_first_test_coherence
|
||||
% #+caption: Coherence
|
||||
% #+RESULTS:
|
||||
% [[file:figs/iff_first_test_coherence.png]]
|
||||
|
||||
% The obtained transfer functions are shown in Figure [[fig:iff_first_test_bode_plot]].
|
||||
% It is clear that the IFF architecture can actively damp the main resonance of the system.
|
||||
|
||||
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2, 1]);
|
||||
hold on;
|
||||
plot(f, abs(tf_iff_of), '-', 'DisplayName', 'g=0')
|
||||
plot(f, abs(tf_iff_g10), '-', 'DisplayName', 'g=10')
|
||||
plot(f, abs(tf_iff_g100), '-', 'DisplayName', 'g=100')
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
|
||||
ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
|
||||
hold off;
|
||||
legend();
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
plot(f, 180/pi*angle(-tf_iff_of), '-')
|
||||
plot(f, 180/pi*angle(-tf_iff_g10), '-')
|
||||
plot(f, 180/pi*angle(-tf_iff_g100), '-')
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
|
||||
ylabel('Phase'); xlabel('Frequency [Hz]');
|
||||
hold off;
|
||||
yticks(-360:90:360);
|
||||
|
||||
linkaxes([ax1,ax2], 'x');
|
||||
xlim([60, 600]);
|
||||
|
||||
% Second test with many Gains
|
||||
% Then, the same identification test is performed for many more gains.
|
||||
|
||||
load('apa95ml_iff_test.mat', 'results');
|
||||
|
||||
Ts = 1e-4;
|
||||
win = hann(ceil(10/Ts));
|
||||
|
||||
tf_iff = {zeros(1, length(results))};
|
||||
co_iff = {zeros(1, length(results))};
|
||||
g_iff = [0, 1, 5, 10, 50, 100];
|
||||
|
||||
for i=1:length(results)
|
||||
[tf_est, f] = tfestimate(results{i}.u, results{i}.y, win, [], [], 1/Ts);
|
||||
[co_est, ~] = mscohere(results{i}.u, results{i}.y, win, [], [], 1/Ts);
|
||||
|
||||
tf_iff(i) = {tf_est};
|
||||
co_iff(i) = {co_est};
|
||||
end
|
||||
|
||||
|
||||
|
||||
% The obtained dynamics are shown in Figure [[fig:iff_results_bode_plots]].
|
||||
|
||||
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2, 1]);
|
||||
hold on;
|
||||
for i = 1:length(results)
|
||||
plot(f, abs(tf_iff{i}), '-', 'DisplayName', sprintf('g = %0.f', g_iff(i)))
|
||||
end
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
|
||||
ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
|
||||
hold off;
|
||||
legend();
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
for i = 1:length(results)
|
||||
plot(f, 180/pi*angle(-tf_iff{i}), '-')
|
||||
end
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
|
||||
ylabel('Phase'); xlabel('Frequency [Hz]');
|
||||
yticks(-360:90:360);
|
||||
axis padded 'auto x'
|
||||
|
||||
linkaxes([ax1,ax2], 'x');
|
||||
xlim([60, 600]);
|
||||
|
||||
|
||||
|
||||
% #+name: fig:iff_results_bode_plots
|
||||
% #+caption: Identified dynamics from excitation voltage to the mass displacement
|
||||
% #+RESULTS:
|
||||
% [[file:figs/iff_results_bode_plots.png]]
|
||||
|
||||
% For each gain, the parameters of a second order resonant system that best fits the data are estimated and are compared with the data in Figure [[fig:iff_results_bode_plots_identification]].
|
||||
|
||||
|
||||
G_id = {zeros(1,length(results))};
|
||||
|
||||
f_start = 70; % [Hz]
|
||||
f_end = 500; % [Hz]
|
||||
|
||||
for i = 1:length(results)
|
||||
tf_id = tf_iff{i}(sum(f<f_start):length(f)-sum(f>f_end));
|
||||
f_id = f(sum(f<f_start):length(f)-sum(f>f_end));
|
||||
|
||||
gfr = idfrd(tf_id, 2*pi*f_id, Ts);
|
||||
G_id(i) = {procest(gfr,'P2UDZ')};
|
||||
end
|
||||
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2, 1]);
|
||||
hold on;
|
||||
for i = 1:length(results)
|
||||
set(gca,'ColorOrderIndex',i)
|
||||
plot(f, abs(tf_iff{i}), '-', 'DisplayName', sprintf('g = %0.f', g_iff(i)))
|
||||
set(gca,'ColorOrderIndex',i)
|
||||
plot(f, abs(squeeze(freqresp(G_id{i}, f, 'Hz'))), '--', 'HandleVisibility', 'off')
|
||||
end
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
|
||||
ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
|
||||
hold off;
|
||||
legend();
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
for i = 1:length(results)
|
||||
set(gca,'ColorOrderIndex',i)
|
||||
plot(f, 180/pi*angle(tf_iff{i}), '-')
|
||||
set(gca,'ColorOrderIndex',i)
|
||||
plot(f, 180/pi*angle(squeeze(freqresp(G_id{i}, f, 'Hz'))), '--')
|
||||
end
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
|
||||
ylabel('Phase'); xlabel('Frequency [Hz]');
|
||||
hold off;
|
||||
yticks(-360:90:360);
|
||||
axis padded 'auto x'
|
||||
|
||||
linkaxes([ax1,ax2], 'x');
|
||||
xlim([60, 600]);
|
||||
|
||||
|
||||
|
||||
% #+name: fig:iff_results_bode_plots_identification
|
||||
% #+caption: Comparison of the measured dynamic and the identified 2nd order resonant systems that best fits the data
|
||||
% #+RESULTS:
|
||||
% [[file:figs/iff_results_bode_plots_identification.png]]
|
||||
|
||||
% Finally, we can represent the position of the poles of the 2nd order systems on the Root Locus plot (Figure [[fig:iff_results_root_locus]]).
|
||||
|
||||
|
||||
w_z = 2*pi*111; % Zeros frequency [rad/s]
|
||||
w_p = 2*pi*255; % Pole frequency [rad/s]
|
||||
xi_z = 0.05;
|
||||
xi_p = 0.015;
|
||||
G_inf = 2;
|
||||
|
||||
Gi = G_inf*(s^2 - 2*xi_z*w_z*s + w_z^2)/(s^2 + 2*xi_p*w_p*s + w_p^2);
|
||||
|
||||
|
||||
gains = logspace(0, 5, 1000);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(real(pole(Gi)), imag(pole(Gi)), 'kx', 'HandleVisibility', 'off');
|
||||
plot(real(tzero(Gi)), imag(tzero(Gi)), 'ko', 'HandleVisibility', 'off');
|
||||
for i = 1:length(results)
|
||||
set(gca,'ColorOrderIndex',i)
|
||||
plot(real(pole(G_id{i})), imag(pole(G_id{i})), 'o', 'DisplayName', sprintf('g = %0.f', g_iff(i)));
|
||||
end
|
||||
for i = 1:length(gains)
|
||||
cl_poles = pole(feedback(Gi, (gains(i)/(s + 2*pi*2))));
|
||||
plot(real(cl_poles), imag(cl_poles), 'k.', 'HandleVisibility', 'off');
|
||||
end
|
||||
ylim([0, 1800]);
|
||||
xlim([-1600,200]);
|
||||
xlabel('Real Part')
|
||||
ylabel('Imaginary Part')
|
||||
axis square
|
||||
legend('location', 'northwest');
|
||||
Binary file not shown.
100
matlab/motion_identification.m
Normal file
100
matlab/motion_identification.m
Normal file
@@ -0,0 +1,100 @@
|
||||
%% Clear Workspace and Close figures
|
||||
clear; close all; clc;
|
||||
|
||||
%% Intialize Laplace variable
|
||||
s = zpk('s');
|
||||
|
||||
addpath('./mat/');
|
||||
|
||||
% Load Data
|
||||
% The data from the "noise test" and the identification test are loaded.
|
||||
|
||||
ht = load('huddle_test.mat', 't', 'y');
|
||||
load('apa95ml_5kg_Amp_E505.mat', 't', 'um', 'y');
|
||||
|
||||
|
||||
|
||||
% Any offset value is removed:
|
||||
|
||||
um = detrend(um, 0); % Generated DAC Voltage [V]
|
||||
y = detrend(y , 0); % Mass displacement [m]
|
||||
|
||||
ht.y = detrend(ht.y, 0);
|
||||
|
||||
|
||||
|
||||
% Now we add a factor 10 to take into account the gain of the voltage amplifier.
|
||||
|
||||
Va = 10*um;
|
||||
|
||||
% Comparison of the PSD with Huddle Test
|
||||
|
||||
Ts = t(end)/(length(t)-1);
|
||||
Fs = 1/Ts;
|
||||
|
||||
win = hanning(ceil(1*Fs));
|
||||
|
||||
[pxx, f] = pwelch(y, win, [], [], Fs);
|
||||
[pht, ~] = pwelch(ht.y, win, [], [], Fs);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(f, sqrt(pxx), 'DisplayName', '5kg');
|
||||
plot(f, sqrt(pht), 'DisplayName', 'Huddle Test');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('ASD [$m/\sqrt{Hz}$]');
|
||||
legend('location', 'northeast');
|
||||
xlim([1, Fs/2]);
|
||||
|
||||
% Compute TF estimate and Coherence
|
||||
|
||||
[tf_est, f] = tfestimate(Va, -y, win, [], [], 1/Ts);
|
||||
[co_est, ~] = mscohere( Va, -y, win, [], [], 1/Ts);
|
||||
|
||||
figure;
|
||||
|
||||
hold on;
|
||||
plot(f, co_est, 'k-')
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
|
||||
ylabel('Coherence'); xlabel('Frequency [Hz]');
|
||||
hold off;
|
||||
xlim([10, 5e3]);
|
||||
|
||||
|
||||
|
||||
% #+name: fig:apa95ml_5kg_PI_coh
|
||||
% #+caption: Coherence
|
||||
% #+RESULTS:
|
||||
% [[file:figs/apa95ml_5kg_PI_coh.png]]
|
||||
|
||||
% Comparison with the FEM model
|
||||
|
||||
load('mat/fem_simscape_models.mat', 'Ghm');
|
||||
|
||||
freqs = logspace(0, 4, 1000);
|
||||
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(f, abs(tf_est), 'DisplayName', 'Identification')
|
||||
plot(freqs, abs(squeeze(freqresp(Ghm, freqs, 'Hz'))), 'DisplayName', 'FEM')
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
|
||||
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel', []);
|
||||
legend('location', 'northeast')
|
||||
hold off;
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
plot(f, 180/pi*angle(tf_est))
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Ghm, freqs, 'Hz'))))
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
|
||||
ylabel('Phase'); xlabel('Frequency [Hz]');
|
||||
hold off;
|
||||
ylim([-180, 180]);
|
||||
yticks(-180:90:180);
|
||||
|
||||
linkaxes([ax1,ax2], 'x');
|
||||
xlim([10, 5e3]);
|
||||
Binary file not shown.
@@ -38,7 +38,7 @@ ka = 235e6; % [N/m]
|
||||
Lmax = 20e-6; % [m]
|
||||
Vmax = 170; % [V]
|
||||
|
||||
ka*Lmax/Vmax % [N/V]
|
||||
ga = ka*Lmax/Vmax; % [N/V]
|
||||
|
||||
% Estimation from Piezoelectric parameters
|
||||
% <<sec:estimation_piezo_params>>
|
||||
|
||||
120
matlab/simscape_model.m
Normal file
120
matlab/simscape_model.m
Normal file
@@ -0,0 +1,120 @@
|
||||
%% Clear Workspace and Close figures
|
||||
clear; close all; clc;
|
||||
|
||||
%% Intialize Laplace variable
|
||||
s = zpk('s');
|
||||
|
||||
addpath('./mat/');
|
||||
|
||||
% Import Mass Matrix, Stiffness Matrix, and Interface Nodes Coordinates
|
||||
% We first extract the stiffness and mass matrices.
|
||||
|
||||
K = readmatrix('APA95ML_K.CSV');
|
||||
M = readmatrix('APA95ML_M.CSV');
|
||||
|
||||
|
||||
|
||||
% #+caption: First 10x10 elements of the Mass matrix
|
||||
% #+RESULTS:
|
||||
% | 0.03 | 7e-08 | 2e-06 | -3e-09 | -0.0002 | -6e-08 | -0.001 | 8e-07 | 6e-07 | -8e-09 |
|
||||
% | 7e-08 | 0.02 | -1e-06 | 9e-05 | -3e-09 | -4e-09 | -1e-06 | -0.0006 | -4e-08 | 5e-06 |
|
||||
% | 2e-06 | -1e-06 | 0.02 | -3e-08 | -4e-08 | 1e-08 | 1e-07 | -2e-07 | 0.0003 | 1e-09 |
|
||||
% | -3e-09 | 9e-05 | -3e-08 | 1e-06 | -3e-11 | -3e-13 | -7e-09 | -5e-06 | -3e-10 | 3e-08 |
|
||||
% | -0.0002 | -3e-09 | -4e-08 | -3e-11 | 2e-06 | 6e-10 | 2e-06 | -7e-09 | -2e-09 | 7e-11 |
|
||||
% | -6e-08 | -4e-09 | 1e-08 | -3e-13 | 6e-10 | 1e-06 | 1e-08 | 3e-09 | -2e-09 | 2e-13 |
|
||||
% | -0.001 | -1e-06 | 1e-07 | -7e-09 | 2e-06 | 1e-08 | 0.03 | 4e-08 | -2e-06 | 8e-09 |
|
||||
% | 8e-07 | -0.0006 | -2e-07 | -5e-06 | -7e-09 | 3e-09 | 4e-08 | 0.02 | -9e-07 | -9e-05 |
|
||||
% | 6e-07 | -4e-08 | 0.0003 | -3e-10 | -2e-09 | -2e-09 | -2e-06 | -9e-07 | 0.02 | 2e-08 |
|
||||
% | -8e-09 | 5e-06 | 1e-09 | 3e-08 | 7e-11 | 2e-13 | 8e-09 | -9e-05 | 2e-08 | 1e-06 |
|
||||
|
||||
% Then, we extract the coordinates of the interface nodes.
|
||||
|
||||
[int_xyz, int_i, n_xyz, n_i, nodes] = extractNodes('APA95ML_out_nodes_3D.txt');
|
||||
|
||||
% Simscape Model
|
||||
% The flexible element is imported using the =Reduced Order Flexible Solid= Simscape block.
|
||||
|
||||
% To model the actuator, an =Internal Force= block is added between the nodes 3 and 12.
|
||||
% A =Relative Motion Sensor= block is added between the nodes 1 and 2 to measure the displacement and the amplified piezo.
|
||||
|
||||
% One mass is fixed at one end of the piezo-electric stack actuator, the other end is fixed to the world frame.
|
||||
|
||||
m = 5.5;
|
||||
|
||||
load('apa95ml_params.mat', 'ga', 'gs');
|
||||
|
||||
% Dynamics from Actuator Voltage to Vertical Mass Displacement
|
||||
% The identified dynamics is shown in Figure [[fig:dynamics_act_disp_comp_mass]].
|
||||
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'piezo_amplified_3d';
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/Va'], 1, 'openinput'); io_i = io_i + 1; % Actuator Voltage [V]
|
||||
io(io_i) = linio([mdl, '/y'], 1, 'openoutput'); io_i = io_i + 1; % Vertical Displacement [m]
|
||||
|
||||
Ghm = linearize(mdl, io);
|
||||
|
||||
freqs = logspace(0, 4, 5000);
|
||||
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(Ghm, freqs, 'Hz'))));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude $d/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
|
||||
hold off;
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Ghm, freqs, 'Hz')))));
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
yticks(-360:90:360);
|
||||
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||||
hold off;
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
|
||||
% Dynamics from Actuator Voltage to Force Sensor Voltage
|
||||
% The obtained dynamics is shown in Figure [[fig:dynamics_force_force_sensor_comp_mass]].
|
||||
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'piezo_amplified_3d';
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/Va'], 1, 'openinput'); io_i = io_i + 1; % Voltage Actuator [V]
|
||||
io(io_i) = linio([mdl, '/Vs'], 1, 'openoutput'); io_i = io_i + 1; % Sensor Voltage [V]
|
||||
|
||||
Gfm = linearize(mdl, io);
|
||||
|
||||
freqs = logspace(1, 5, 1000);
|
||||
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(Gfm, freqs, 'Hz'))));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude $V_s/V_a$ [V/V]'); set(gca, 'XTickLabel',[]);
|
||||
hold off;
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gfm, freqs, 'Hz')))));
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
yticks(-360:90:360); ylim([-360, 180]);
|
||||
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||||
hold off;
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
|
||||
save('mat/fem_simscape_models.mat', 'Ghm', 'Gfm')
|
||||
Reference in New Issue
Block a user