281 lines
8.2 KiB
Matlab
281 lines
8.2 KiB
Matlab
%% Clear Workspace and Close figures
|
|
clear; close all; clc;
|
|
|
|
%% Intialize Laplace variable
|
|
s = zpk('s');
|
|
|
|
addpath('active_damping/src/');
|
|
|
|
open('active_damping/matlab/sim_nass_active_damping.slx')
|
|
|
|
load('./active_damping/mat/undamped_plants.mat', 'G_iff');
|
|
load('./active_damping/mat/plants_variable.mat', 'masses', 'Gm_iff');
|
|
|
|
freqs = logspace(-2, 3, 1000);
|
|
|
|
figure;
|
|
|
|
ax1 = subplot(2, 1, 1);
|
|
hold on;
|
|
for i=1:length(masses)
|
|
plot(freqs, abs(squeeze(freqresp(-Gm_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
|
|
|
|
ax2 = subplot(2, 1, 2);
|
|
hold on;
|
|
for i=1:length(masses)
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(-Gm_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), ...
|
|
'DisplayName', sprintf('$M = %.0f$ [kg]', masses(i)));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
|
ylim([-180, 180]);
|
|
yticks([-180, -90, 0, 90, 180]);
|
|
legend('location', 'southwest');
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
|
|
w0 = 2*pi*50;
|
|
K_iff = -5000/s * (s/w0)/(1 + s/w0);
|
|
|
|
freqs = logspace(0, 3, 1000);
|
|
|
|
figure;
|
|
|
|
ax1 = subplot(2, 1, 1);
|
|
hold on;
|
|
for i=1:length(masses)
|
|
plot(freqs, abs(squeeze(freqresp(K_iff*Gm_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
|
|
|
|
ax2 = subplot(2, 1, 2);
|
|
hold on;
|
|
for i=1:length(masses)
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(K_iff*Gm_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), ...
|
|
'DisplayName', sprintf('$M = %.0f$ [kg]', masses(i)));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
|
ylim([-180, 180]);
|
|
yticks([-180, -90, 0, 90, 180]);
|
|
legend('location', 'southwest');
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
|
|
K_iff = -K_iff*eye(6);
|
|
|
|
save('./active_damping/mat/K_iff.mat', 'K_iff');
|
|
|
|
prepareLinearizeIdentification();
|
|
|
|
load('./active_damping/mat/K_iff.mat', 'K_iff');
|
|
initializeController('type', 'iff', 'K', K_iff);
|
|
|
|
%% Options for Linearized
|
|
options = linearizeOptions;
|
|
options.SampleTime = 0;
|
|
|
|
%% Name of the Simulink File
|
|
mdl = 'sim_nass_active_damping';
|
|
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Fnl'], 1, 'openinput'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/Compute Error in NASS base'], 2, 'openoutput'); io_i = io_i + 1;
|
|
|
|
load('./active_damping/mat/cart_plants.mat', 'masses');
|
|
|
|
G_cart_iff = {zeros(length(masses))};
|
|
|
|
load('mat/stages.mat', 'nano_hexapod');
|
|
|
|
for i = 1:length(masses)
|
|
initializeSample('mass', masses(i));
|
|
|
|
%% Run the linearization
|
|
G = linearize(mdl, io, 0.3, options);
|
|
G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
|
|
G.OutputName = {'Dnx', 'Dny', 'Dnz', 'Rnx', 'Rny', 'Rnz'};
|
|
|
|
G_cart = G*inv(nano_hexapod.J');
|
|
G_cart.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'};
|
|
|
|
G_cart_iff(i) = {G_cart};
|
|
end
|
|
|
|
save('./active_damping/mat/cart_plants.mat', 'G_cart_iff', '-append');
|
|
|
|
load('./active_damping/mat/cart_plants.mat', 'masses', 'G_cart', 'G_cart_iff');
|
|
|
|
freqs = logspace(0, 3, 1000);
|
|
|
|
figure;
|
|
|
|
ax1 = subplot(2, 1, 1);
|
|
hold on;
|
|
for i = 1:length(masses)
|
|
set(gca,'ColorOrderIndex',i);
|
|
p1 = plot(freqs, abs(squeeze(freqresp(G_cart_iff{i}('Dnx', 'Fnx'), freqs, 'Hz'))));
|
|
set(gca,'ColorOrderIndex',i);
|
|
p2 = plot(freqs, abs(squeeze(freqresp(G_cart_iff{i}('Dny', 'Fny'), freqs, 'Hz'))), '--');
|
|
set(gca,'ColorOrderIndex',i);
|
|
p3 = plot(freqs, abs(squeeze(freqresp(G_cart_iff{i}('Dnz', 'Fnz'), freqs, 'Hz'))), ':');
|
|
end
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
|
|
legend([p1,p2,p3], {'Fx/Dx', 'Fy/Dx', 'Fz/Dz'});
|
|
|
|
ax2 = subplot(2, 1, 2);
|
|
hold on;
|
|
for i = 1:length(masses)
|
|
set(gca,'ColorOrderIndex',i);
|
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_cart_iff{i}('Dnx', 'Fnx'), freqs, 'Hz')))), ...
|
|
'DisplayName', sprintf('$M = %.0f$ [kg]', masses(i)));
|
|
set(gca,'ColorOrderIndex',i);
|
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_cart_iff{i}('Dny', 'Fny'), freqs, 'Hz')))), '--', 'HandleVisibility', 'off');
|
|
set(gca,'ColorOrderIndex',i);
|
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_cart_iff{i}('Dnz', 'Fnz'), freqs, 'Hz')))), ':', 'HandleVisibility', 'off');
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
|
yticks([-540:180:540]);
|
|
legend('location', 'northeast');
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
|
|
freqs = logspace(0, 3, 1000);
|
|
|
|
figure;
|
|
|
|
ax1 = subplot(2, 1, 1);
|
|
hold on;
|
|
for i = 1:length(masses)
|
|
set(gca,'ColorOrderIndex',i);
|
|
p1 = plot(freqs, abs(squeeze(freqresp(G_cart_iff{i}('Rnx', 'Mnx'), freqs, 'Hz'))));
|
|
set(gca,'ColorOrderIndex',i);
|
|
p2 = plot(freqs, abs(squeeze(freqresp(G_cart_iff{i}('Rny', 'Mny'), freqs, 'Hz'))), '--');
|
|
set(gca,'ColorOrderIndex',i);
|
|
p3 = plot(freqs, abs(squeeze(freqresp(G_cart_iff{i}('Rnz', 'Mnz'), freqs, 'Hz'))), ':');
|
|
end
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
|
|
legend([p1,p2,p3], {'Fx/Dx', 'Fy/Dx', 'Fz/Dz'});
|
|
|
|
ax2 = subplot(2, 1, 2);
|
|
hold on;
|
|
for i = 1:length(masses)
|
|
set(gca,'ColorOrderIndex',i);
|
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_cart_iff{i}('Rnx', 'Mnx'), freqs, 'Hz')))), ...
|
|
'DisplayName', sprintf('$M = %.0f$ [kg]', masses(i)));
|
|
set(gca,'ColorOrderIndex',i);
|
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_cart_iff{i}('Rny', 'Mny'), freqs, 'Hz')))), '--', 'HandleVisibility', 'off');
|
|
set(gca,'ColorOrderIndex',i);
|
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_cart_iff{i}('Rnz', 'Mnz'), freqs, 'Hz')))), ':', 'HandleVisibility', 'off');
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
|
yticks([-540:180:540]);
|
|
legend('location', 'northeast');
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
|
|
freqs = logspace(1, 3, 1000);
|
|
|
|
figure;
|
|
|
|
for ix = 1:6
|
|
for iy = 1:6
|
|
subplot(6, 6, (ix-1)*6 + iy);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G_cart{1}(ix, iy), freqs, 'Hz'))), 'k-');
|
|
plot(freqs, abs(squeeze(freqresp(G_cart_iff{1}(ix, iy), freqs, 'Hz'))), 'k--');
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylim([1e-13, 1e-4]);
|
|
xticks([])
|
|
yticks([])
|
|
end
|
|
end
|
|
|
|
prepareTomographyExperiment();
|
|
|
|
load('./active_damping/mat/K_iff.mat', 'K_iff');
|
|
initializeController('type', 'iff', 'K', K_iff);
|
|
|
|
load('mat/conf_simulink.mat');
|
|
set_param(conf_simulink, 'StopTime', '4.5');
|
|
|
|
sim('sim_nass_active_damping');
|
|
|
|
En_iff = En;
|
|
Eg_iff = Eg;
|
|
save('./active_damping/mat/tomo_exp.mat', 'En_iff', 'Eg_iff', '-append');
|
|
|
|
load('./active_damping/mat/tomo_exp.mat', 'En', 'En_iff');
|
|
Fs = 1e3; % Sampling Frequency of the Data
|
|
t = (1/Fs)*[0:length(En(:,1))-1];
|
|
|
|
figure;
|
|
hold on;
|
|
plot(En(:,1), En(:,2), 'DisplayName', '$\epsilon_{x,y}$ - OL')
|
|
plot(En_iff(:,1), En_iff(:,2), 'DisplayName', '$\epsilon_{x,y}$ - IFF')
|
|
xlabel('X Motion [m]'); ylabel('Y Motion [m]');
|
|
legend('location', 'northwest');
|
|
|
|
figure;
|
|
ax1 = subplot(3, 1, 1);
|
|
hold on;
|
|
plot(t, En(:,1), 'DisplayName', '$\epsilon_{x}$')
|
|
plot(t, En_iff(:,1), 'DisplayName', '$\epsilon_{x}$ - IFF')
|
|
legend('location', 'southwest');
|
|
|
|
ax2 = subplot(3, 1, 2);
|
|
hold on;
|
|
plot(t, En(:,2), 'DisplayName', '$\epsilon_{y}$')
|
|
plot(t, En_iff(:,2), 'DisplayName', '$\epsilon_{y}$ - IFF')
|
|
legend('location', 'southwest');
|
|
ylabel('Position Error [m]');
|
|
|
|
ax3 = subplot(3, 1, 3);
|
|
hold on;
|
|
plot(t, En(:,3), 'DisplayName', '$\epsilon_{z}$')
|
|
plot(t, En_iff(:,3), 'DisplayName', '$\epsilon_{z}$ - IFF')
|
|
legend('location', 'northwest');
|
|
xlabel('Time [s]');
|
|
|
|
linkaxes([ax1,ax2,ax3],'x');
|
|
xlim([0.5,inf]);
|
|
|
|
figure;
|
|
ax1 = subplot(3, 1, 1);
|
|
hold on;
|
|
plot(t, En(:,4), 'DisplayName', '$\epsilon_{\theta_x}$')
|
|
plot(t, En_iff(:,4), 'DisplayName', '$\epsilon_{\theta_x}$ - IFF')
|
|
legend('location', 'northwest');
|
|
|
|
ax2 = subplot(3, 1, 2);
|
|
hold on;
|
|
plot(t, En(:,5), 'DisplayName', '$\epsilon_{\theta_y}$')
|
|
plot(t, En_iff(:,5), 'DisplayName', '$\epsilon_{\theta_y}$ - IFF')
|
|
legend('location', 'southwest');
|
|
ylabel('Position Error [rad]');
|
|
|
|
ax3 = subplot(3, 1, 3);
|
|
hold on;
|
|
plot(t, En(:,6), 'DisplayName', '$\epsilon_{\theta_z}$')
|
|
plot(t, En_iff(:,6), 'DisplayName', '$\epsilon_{\theta_z}$ - IFF')
|
|
legend();
|
|
xlabel('Time [s]');
|
|
|
|
linkaxes([ax1,ax2,ax3],'x');
|
|
xlim([0.5,inf]);
|