117 lines
4.7 KiB
Matlab
117 lines
4.7 KiB
Matlab
%%
|
|
clear; close all; clc;
|
|
|
|
%% Load plant and controller
|
|
load('./mat/G_iff.mat', 'G_iff_light_vc', 'G_iff_light_pz', 'G_iff_heavy_vc', 'G_iff_heavy_pz');
|
|
load('./mat/K_fb_iff.mat', 'K_light_vc_iff', 'K_light_pz_iff', 'K_heavy_vc_iff', 'K_heavy_pz_iff');
|
|
|
|
%% Load Configuration
|
|
load('./mat/config.mat', 'save_fig', 'freqs');
|
|
|
|
%% Plot the Loop gain for Translations - Light VC
|
|
figure;
|
|
% Amplitude
|
|
ax1 = subplot(2,1,1);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(K_light_vc_iff(1, 1)*G_iff_light_vc.G_cart(1, 1), freqs, 'Hz'))), 'DisplayName', 'x');
|
|
plot(freqs, abs(squeeze(freqresp(K_light_vc_iff(2, 2)*G_iff_light_vc.G_cart(2, 2), freqs, 'Hz'))), 'DisplayName', 'y');
|
|
plot(freqs, abs(squeeze(freqresp(K_light_vc_iff(3, 3)*G_iff_light_vc.G_cart(3, 3), freqs, 'Hz'))), 'DisplayName', 'z');
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
set(gca, 'XTickLabel',[]);
|
|
ylabel('Amplitude [m/N]');
|
|
hold off;
|
|
% Phase
|
|
ax2 = subplot(2,1,2);
|
|
hold on;
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(K_light_vc_iff(1, 1)*G_iff_light_vc.G_cart(1, 1), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(K_light_vc_iff(2, 2)*G_iff_light_vc.G_cart(2, 2), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(K_light_vc_iff(3, 3)*G_iff_light_vc.G_cart(3, 3), freqs, 'Hz'))));
|
|
set(gca,'xscale','log');
|
|
yticks(-180:90:180);
|
|
ylim([-180 180]);
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
legend('Location', 'southwest');
|
|
hold off;
|
|
linkaxes([ax1,ax2],'x');
|
|
|
|
if save_fig; exportFig('loop_gain_fb_iff_light_vc_trans', 'normal-normal', struct('path', 'active_damping')); end
|
|
|
|
%% Plot the Loop gain for Rotations - Light VC
|
|
figure;
|
|
% Amplitude
|
|
ax1 = subplot(2,1,1);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(K_light_vc_iff(4, 4)*G_iff_light_vc.G_cart(4, 4), freqs, 'Hz'))), 'DisplayName', 'Rx');
|
|
plot(freqs, abs(squeeze(freqresp(K_light_vc_iff(5, 5)*G_iff_light_vc.G_cart(5, 5), freqs, 'Hz'))), 'DisplayName', 'Ry');
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
set(gca, 'XTickLabel',[]);
|
|
ylabel('Amplitude [m/N]');
|
|
hold off;
|
|
% Phase
|
|
ax2 = subplot(2,1,2);
|
|
hold on;
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(K_light_vc_iff(4, 4)*G_iff_light_vc.G_cart(4, 4), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(K_light_vc_iff(5, 5)*G_iff_light_vc.G_cart(5, 5), freqs, 'Hz'))));
|
|
set(gca,'xscale','log');
|
|
yticks(-180:90:180);
|
|
ylim([-180 180]);
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
legend('Location', 'southwest');
|
|
hold off;
|
|
linkaxes([ax1,ax2],'x');
|
|
|
|
if save_fig; exportFig('loop_gain_fb_iff_light_vc_rot', 'normal-normal', struct('path', 'active_damping')); end
|
|
|
|
%% Plot the Loop gain for Translations - Light PZ
|
|
figure;
|
|
% Amplitude
|
|
ax1 = subplot(2,1,1);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(K_light_pz_iff(1, 1)*G_iff_light_pz.G_cart(1, 1), freqs, 'Hz'))), 'DisplayName', 'x');
|
|
plot(freqs, abs(squeeze(freqresp(K_light_pz_iff(2, 2)*G_iff_light_pz.G_cart(2, 2), freqs, 'Hz'))), 'DisplayName', 'y');
|
|
plot(freqs, abs(squeeze(freqresp(K_light_pz_iff(3, 3)*G_iff_light_pz.G_cart(3, 3), freqs, 'Hz'))), 'DisplayName', 'z');
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
set(gca, 'XTickLabel',[]);
|
|
ylabel('Amplitude [m/N]');
|
|
hold off;
|
|
% Phase
|
|
ax2 = subplot(2,1,2);
|
|
hold on;
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(K_light_pz_iff(1, 1)*G_iff_light_pz.G_cart(1, 1), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(K_light_pz_iff(2, 2)*G_iff_light_pz.G_cart(2, 2), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(K_light_pz_iff(3, 3)*G_iff_light_pz.G_cart(3, 3), freqs, 'Hz'))));
|
|
set(gca,'xscale','log');
|
|
yticks(-180:90:180);
|
|
ylim([-180 180]);
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
legend('Location', 'southwest');
|
|
hold off;
|
|
linkaxes([ax1,ax2],'x');
|
|
|
|
if save_fig; exportFig('loop_gain_fb_iff_light_pz_trans', 'normal-normal', struct('path', 'active_damping')); end
|
|
|
|
%% Plot the Loop gain for Rotations - Light PZ
|
|
figure;
|
|
% Amplitude
|
|
ax1 = subplot(2,1,1);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(K_light_pz_iff(4, 4)*G_iff_light_pz.G_cart(4, 4), freqs, 'Hz'))), 'DisplayName', 'Rx');
|
|
plot(freqs, abs(squeeze(freqresp(K_light_pz_iff(5, 5)*G_iff_light_pz.G_cart(5, 5), freqs, 'Hz'))), 'DisplayName', 'Ry');
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
set(gca, 'XTickLabel',[]);
|
|
ylabel('Amplitude [m/N]');
|
|
hold off;
|
|
% Phase
|
|
ax2 = subplot(2,1,2);
|
|
hold on;
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(K_light_pz_iff(4, 4)*G_iff_light_pz.G_cart(4, 4), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(K_light_pz_iff(5, 5)*G_iff_light_pz.G_cart(5, 5), freqs, 'Hz'))));
|
|
set(gca,'xscale','log');
|
|
yticks(-180:90:180);
|
|
ylim([-180 180]);
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
legend('Location', 'southwest');
|
|
hold off;
|
|
linkaxes([ax1,ax2],'x');
|
|
|
|
if save_fig; exportFig('loop_gain_fb_iff_light_pz_rot', 'normal-normal', struct('path', 'active_damping')); end |