add all files
This commit is contained in:
318
B2-nass-fem/detail_fem_2_actuators.m
Normal file
318
B2-nass-fem/detail_fem_2_actuators.m
Normal file
@@ -0,0 +1,318 @@
|
||||
%% 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
Block a user