tdehaeze/nass-simscape
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Huge Change

Browse Source 
- Add may folders
- Add IFF and HAC-LAC scripts
This commit is contained in:
Thomas Dehaeze
2018-10-07 22:07:21 +02:00
parent 496dd15586
commit f0e83d7c39
84 changed files with 1292 additions and 425 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
hac_lac/hac_lac_iff_control.m Normal file
View File

@@ -0,0 +1,17 @@
%%
clear; close all; clc;
%% Load Plant
load('./mat/G_iff.mat', 'G_iff_light_vc', 'G_iff_light_pz', 'G_iff_heavy_vc', 'G_iff_heavy_pz');
%%
fs = 10;
K_light_vc_iff = generateDiagPidControl(G_iff_light_vc.G_cart, fs);
K_light_pz_iff = generateDiagPidControl(G_iff_light_pz.G_cart, fs);
K_heavy_vc_iff = generateDiagPidControl(G_iff_heavy_vc.G_cart, fs);
K_heavy_pz_iff = generateDiagPidControl(G_iff_heavy_pz.G_cart, fs);
%% Save the MIMO control
save('./mat/K_fb_iff.mat', 'K_light_vc_iff', 'K_light_pz_iff', 'K_heavy_vc_iff', 'K_heavy_pz_iff');

117
hac_lac/hac_lac_iff_control_plots.m Normal file
View File

@@ -0,0 +1,117 @@
%%
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 = subaxis(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 = subaxis(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 = subaxis(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 = subaxis(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 = subaxis(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 = subaxis(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 = subaxis(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 = subaxis(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

11
hac_lac/hac_lac_iff_simulation.m Normal file
View File

@@ -0,0 +1,11 @@
%%
clear; close all; clc;
%% Initialize Simulation and Inputs
initializeExperiment('tomography', 'light');
%% Run Closed Loop Simulations
runSimulation('vc', 'light', 'cl', 'iff');
runSimulation('pz', 'light', 'cl', 'iff');
% runSimulation('vc', 'heavy', 'cl', 'iff');
% runSimulation('pz', 'heavy', 'cl', 'iff');

2
hac_lac/hac_lac_main.m Normal file
View File

@@ -0,0 +1,2 @@
%%
clear; close all; clc;