tdehaeze/phd-nass-fem
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
phd-nass-fem/matlab/detail_fem_2_actuators.m

319 lines
11 KiB
Matlab
Raw Permalink Blame History

%% 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
addpath('./subsystems/'); % Path for Subsystems Simulink files
%% Linearization options
opts = linearizeOptions;
opts.SampleTime = 0;
%% Open Simscape Model
mdl = 'detail_fem_APA300ML'; % Name of the Simulink File
open(mdl); % Open Simscape Model
% Piezoelectric parameters
ga = -25.9; % [N/V]
gs = -5.08e6; % [V/m]
%% Colors for the figures
colors = colororder;
freqs = logspace(1,3,500); % Frequency vector [Hz]
%% Identify dynamics with "Reduced Order Flexible Body"
K = readmatrix('APA300ML_mat_K.CSV');
M = readmatrix('APA300ML_mat_M.CSV');
[int_xyz, int_i, n_xyz, n_i, nodes] = extractNodes('APA300ML_out_nodes_3D.txt');
m = 5; % [kg]
ga = 25.9; % [N/V]
gs = 5.08e6; % [V/m]
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Va'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Fd'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/z'], 1, 'openoutput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Vs'], 1, 'openoutput'); io_i = io_i + 1;
G_fem = linearize(mdl, io);
G_fem_z = G_fem('z','Va');
G_fem_Vs = G_fem('Vs', 'Va');
G_fem_comp = G_fem('z', 'Fd');
%% Determine c1 and k1 from the zero
G_zeros = zero(minreal(G_fem_Vs));
G_zeros = G_zeros(imag(G_zeros)>0);
[~, i_sort] = sort(imag(G_zeros));
G_zeros = G_zeros(i_sort);
G_zero = G_zeros(1);
% Solving 2nd order equations
c1 = -2*m*real(G_zero);
k1 = m*(imag(G_zero)^2 + real(G_zero)^2);
%% Determine ka, ke, ca, ce from the first pole
G_poles = pole(minreal(G_fem_z));
G_poles = G_poles(imag(G_poles)>0);
[~, i_sort] = sort(imag(G_poles));
G_poles = G_poles(i_sort);
G_pole = G_poles(1);
% Solving 2nd order equations
ce = 3*(-2*m*real(G_pole(1)) - c1);
ca = 1/2*ce;
ke = 3*(m*(imag(G_pole)^2 + real(G_pole)^2) - k1);
ka = 1/2*ke;
%% Matching sensor/actuator constants
% ga = dcgain(G_fem_z) / (1/(ka + k1*ke/(k1 + ke)));
clear io; io_i = 1;
io(io_i) = linio([mdl, '_2dof', '/Fa'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '_2dof', '/z'], 1, 'openoutput'); io_i = io_i + 1;
ga = dcgain(G_fem_z)/dcgain(linearize([mdl, '_2dof'], io));
clear io; io_i = 1;
io(io_i) = linio([mdl, '_2dof', '/Va'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '_2dof', '/dL'], 1, 'openoutput'); io_i = io_i + 1;
gs = dcgain(G_fem_Vs)/dcgain(linearize([mdl, '_2dof'], io));
%% Identify dynamics with tuned 2DoF model
clear io; io_i = 1;
io(io_i) = linio([mdl, '_2dof', '/Va'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '_2dof', '/Fd'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '_2dof', '/z'], 1, 'openoutput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '_2dof', '/Vs'], 1, 'openoutput'); io_i = io_i + 1;
G_2dof = linearize([mdl, '_2dof'], io);
G_2dof_z = G_2dof('z','Va');
G_2dof_Vs = G_2dof('Vs', 'Va');
G_2dof_comp = G_2dof('z', 'Fd');
%% Comparison of the transfer functions from Va to vertical motion - FEM vs 2DoF
freqs = logspace(1, 3, 500);
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_fem_z, freqs, 'Hz'))), '-', 'color', [colors(2,:), 0.5], 'linewidth', 2.5, 'DisplayName', 'FEM')
plot(freqs, abs(squeeze(freqresp(G_2dof_z, freqs, 'Hz'))), '--', 'color', colors(2,:), 'DisplayName', '2DoF Model')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $y/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-8, 2e-4]);
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_fem_z, freqs, 'Hz')))), '-', 'color', [colors(2,:), 0.5], 'linewidth', 2.5);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_2dof_z, 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:45:360); ylim([-20, 200]);
linkaxes([ax1,ax2],'x');
xlim([10, 1e3]);
%% Comparison of the transfer functions from Va to Vs - FEM vs 2DoF
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_fem_Vs, freqs, 'Hz'))), '-', 'color', [colors(1,:), 0.5], 'linewidth', 2.5, 'DisplayName', 'FEM');
plot(freqs, abs(squeeze(freqresp(G_2dof_Vs, freqs, 'Hz'))), '--', 'color', colors(1,:), 'DisplayName', '2DoF Model')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_s/V_a$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([6e-4, 3e1]);
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
ax2 = nexttile;
hold on;
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_fem_Vs, freqs, 'Hz')))), '-', 'color', [colors(1,:), 0.5], 'linewidth', 2.5);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_2dof_Vs, 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:45:360); ylim([-20, 200]);
linkaxes([ax1,ax2],'x');
xlim([10, 1e3]);
%% Effect of electrical boundaries on the
oc = load('detail_fem_apa95ml_open_circuit.mat', 't', 'encoder', 'u');
sc = load('detail_fem_apa95ml_short_circuit.mat', 't', 'encoder', 'u');
% Spectral Analysis parameters
Ts = sc.t(end)/(length(sc.t)-1);
Nfft = floor(2/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);
% Identification of the transfer function from Va to di
[G_oc, f] = tfestimate(detrend(oc.u, 0), detrend(oc.encoder, 0), win, Noverlap, Nfft, 1/Ts);
[G_sc, f] = tfestimate(detrend(sc.u, 0), detrend(sc.encoder, 0), win, Noverlap, Nfft, 1/Ts);
% Find resonance frequencies
[~, i_oc] = max(abs(G_oc(f<300)));
[~, i_sc] = max(abs(G_sc(f<300)));
%% Effect of the electrical bondaries of the force sensor stack on the APA95ML resonance frequency
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
plot(f, abs(G_oc), '-', 'DisplayName', sprintf('Open-Circuit - $f_0 = %.1f Hz$', f(i_oc)))
plot(f, abs(G_sc), '-', 'DisplayName', sprintf('Short-Circuit - $f_0 = %.1f Hz$', f(i_sc)))
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-6, 1e-4]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(G_oc), '-')
plot(f, 180/pi*angle(G_sc), '-')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
ylabel('Phase'); xlabel('Frequency [Hz]');
hold off;
yticks(-360:45:360);
ylim([-20, 200]);
axis padded 'auto x'
linkaxes([ax1,ax2], 'x');
xlim([100, 300]);
%% Compare Dynamics between "Reduced Order" flexible joints and "2-dof and 3-dof" joints
% Let's initialize all the stages with default parameters.
initializeGround('type', 'rigid');
initializeGranite('type', 'rigid');
initializeTy('type', 'rigid');
initializeRy('type', 'rigid');
initializeRz('type', 'rigid');
initializeMicroHexapod('type', 'rigid');
initializeSample('m', 50);
initializeSimscapeConfiguration();
initializeDisturbances('enable', false);
initializeLoggingConfiguration('log', 'none');
initializeController('type', 'open-loop');
initializeReferences();
mdl = 'detail_fem_nass';
% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'EdL'); io_i = io_i + 1; % Errors in the frame of the struts
io(io_i) = linio([mdl, '/NASS'], 3, 'openoutput', [], 'fn'); io_i = io_i + 1; % Force Sensors
% Flexible actuators
initializeSimplifiedNanoHexapod('actuator_type', 'flexible', ...
'flex_type_F', '2dof', ...
'flex_type_M', '3dof');
G_flex = linearize(mdl, io);
G_flex.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_flex.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6', 'fm1', 'fm2', 'fm3', 'fm4', 'fm5', 'fm6'};
% Actuators modeled as 2DoF system
initializeSimplifiedNanoHexapod('actuator_type', 'apa300ml', ...
'flex_type_F', '2dof', ...
'flex_type_M', '3dof');
G_ideal = linearize(mdl, io);
G_ideal.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_ideal.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6', 'fm1', 'fm2', 'fm3', 'fm4', 'fm5', 'fm6'};
%% Comparison of the dynamics for actuators modeled using "reduced order flexible body" and using 2DoF system - HAC plant
freqs = logspace(1, 4, 1000);
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
for j = 1:5
for k = j+1:6
plot(freqs, abs(squeeze(freqresp(G_flex("l"+k,"f"+j), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ...
'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_ideal("l"+k,"f"+j), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ...
'HandleVisibility', 'off');
end
end
plot(freqs, abs(squeeze(freqresp(G_flex("l1","f1"), freqs, 'Hz'))), 'color', colors(1,:), 'DisplayName', 'FEM');
plot(freqs, abs(squeeze(freqresp(G_ideal("l1","f1"), freqs, 'Hz'))), 'color', colors(2,:), 'DisplayName', '2-DoF');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
ylim([1e-10, 1e-4]);
ax2 = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G_flex("l1","f1"), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ideal("l1","f1"), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-20, 200]);
yticks([-360:45:360]);
linkaxes([ax1,ax2],'x');
%% Comparison of the dynamics for actuators modeled using "reduced order flexible body" and using 2DoF system - IFF plant
freqs = logspace(0, 3, 1000);
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
for j = 1:5
for k = j+1:6
plot(freqs, abs(squeeze(freqresp(G_flex("fm"+k,"f"+j), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ...
'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_ideal("fm"+k,"f"+j), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ...
'HandleVisibility', 'off');
end
end
plot(freqs, abs(squeeze(freqresp(G_flex("fm1","f1"), freqs, 'Hz'))), 'color', colors(1,:), 'DisplayName', 'FEM');
plot(freqs, abs(squeeze(freqresp(G_ideal("fm1","f1"), freqs, 'Hz'))), 'color', colors(2,:), 'DisplayName', '2-DoF');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
ylim([1e-5, 1e1]);
ax2 = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G_flex("fm1","f1"), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ideal("fm1","f1"), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-20, 200]);
yticks([-360:45:360]);
linkaxes([ax1,ax2],'x');
Reference in New Issue View Git Blame Copy Permalink