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

23
src/computePsdDispl.m Normal file
View File

@@ -0,0 +1,23 @@
function [psd_object] = computePsdDispl(sys_data, t_init, n_av)
i_init = find(sys_data.time > t_init, 1);
han_win = hanning(ceil(length(sys_data.Dx(i_init:end, :))/n_av));
Fs = 1/sys_data.time(2);
[pdx, f] = pwelch(sys_data.Dx(i_init:end, :), han_win, [], [], Fs);
[pdy, ~] = pwelch(sys_data.Dy(i_init:end, :), han_win, [], [], Fs);
[pdz, ~] = pwelch(sys_data.Dz(i_init:end, :), han_win, [], [], Fs);
[prx, ~] = pwelch(sys_data.Rx(i_init:end, :), han_win, [], [], Fs);
[pry, ~] = pwelch(sys_data.Ry(i_init:end, :), han_win, [], [], Fs);
[prz, ~] = pwelch(sys_data.Rz(i_init:end, :), han_win, [], [], Fs);
psd_object = struct(...
'f', f, ...
'dx', pdx, ...
'dy', pdy, ...
'dz', pdz, ...
'rx', prx, ...
'ry', pry, ...
'rz', prz);
end

18
src/generateDiagPidControl.m Normal file
View File

@@ -0,0 +1,18 @@
function [K] = generateDiagPidControl(G, fs)
%%
pid_opts = pidtuneOptions(...
'PhaseMargin', 50, ...
'DesignFocus', 'disturbance-rejection');
%%
K = tf(zeros(6));
for i = 1:5
input_name = G.InputName(i);
output_name = G.OutputName(i);
K(i, i) = tf(pidtune(minreal(G(output_name, input_name)), 'PIDF', 2*pi*fs, pid_opts));
end
K.InputName = G.OutputName;
K.OutputName = G.InputName;
end

28
src/identifyG.m
View File

@@ -1,28 +0,0 @@
function [G, G_raw] = identifyG(opts_param)
%% Default values for opts
opts = struct();
%% Populate opts with input parameters
if exist('opts_param','var')
for opt = fieldnames(opts_param)'
opts.(opt{1}) = opts_param.(opt{1});
end
end
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'sim_nano_station';
%% Centralized control (Cartesian coordinates)
% Input/Output definition
io(1) = linio([mdl, '/Micro-Station/Fn'], 1,'openinput');
io(2) = linio([mdl, '/Micro-Station/Sample'],1,'output');
% Run the linearization
G = linearize(mdl,io, 0);
G.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'};
G.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
end

35
src/identifyGd.m
View File

@@ -1,35 +0,0 @@
function [Gd, Gd_raw] = identifyGd(opts_param)
%% Default values for opts
opts = struct('cl', true);
%% Populate opts with input parameters
if exist('opts_param','var')
for opt = fieldnames(opts_param)'
opts.(opt{1}) = opts_param.(opt{1});
end
end
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
if opts.cl
% Make sure that the loop is closed
initializeSimConf(struct('cl_time', 0));
mdl = 'Assemblage';
else
mdl = 'sim_nano_station';
end
%% Centralized control (Cartesian coordinates)
% Input/Output definition
io(1) = linio([mdl, '/Micro-Station/Gm'], 1,'openinput');
io(2) = linio([mdl, '/Micro-Station/Fs_ext'], 1,'openinput');
io(3) = linio([mdl, '/Micro-Station/Sample'], 1,'output');
% Run the linearization
Gd = linearize(mdl,io, 0);
Gd.InputName = {'Dgx', 'Dgy', 'Dgz', 'Fsx', 'Fsy', 'Fsz'};
Gd.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
end

30
src/identifyNass.m
View File

@@ -1,30 +0,0 @@
function [G_cart, G_cart_raw] = identifyNass(opts_param)
%% Default values for opts
opts = struct();
%% Populate opts with input parameters
if exist('opts_param','var')
for opt = fieldnames(opts_param)'
opts.(opt{1}) = opts_param.(opt{1});
end
end
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'sim_nano_station';
%% Centralized control (Cartesian coordinates)
% Input/Output definition
io(1) = linio([mdl, '/Micro-Station/Fn'], 1, 'openinput');
io(2) = linio([mdl, '/Micro-Station/Nano_Hexapod'], 1, 'output');
% Run the linearization
G_cart = linearize(mdl,io, 0);
% Input/Output names
G_cart.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'};
G_cart.OutputName = {'Dnx', 'Dny', 'Dnz', 'Rnx', 'Rny', 'Rnz'};
end

42
src/identifyPlant.m Normal file
View File

@@ -0,0 +1,42 @@
function [sys] = identifyPlant(opts_param)
%% Default values for opts
opts = struct();
%% Populate opts with input parameters
if exist('opts_param','var')
for opt = fieldnames(opts_param)'
opts.(opt{1}) = opts_param.(opt{1});
end
end
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'sim_nano_station';
%% Input/Output definition
io(1) = linio([mdl, '/Fn'], 1, 'input');
io(2) = linio([mdl, '/Gm'], 1, 'input');
io(3) = linio([mdl, '/Fs_ext'], 1, 'input');
io(4) = linio([mdl, '/F_legs'], 1, 'input');
io(5) = linio([mdl, '/Dsample_meas'], 1, 'output');
io(6) = linio([mdl, '/F_meas'], 1, 'output');
%% Run the linearization
G = linearize(mdl, io, 0);
G.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz', ...
'Dgx', 'Dgy', 'Dgz', ...
'Fsx', 'Fsy', 'Fsz', ...
'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', ...
'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6'};
%% Create the sub transfer functions
sys.G_cart = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'}));
sys.G_gm = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Dgx', 'Dgy', 'Dgz'}));
sys.G_fs = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Fsx', 'Fsy', 'Fsz'}));
sys.G_iff = minreal(G({'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}));
end

56
src/runSimulation.m Normal file
View File

@@ -0,0 +1,56 @@
function [] = runSimulation(sys_name, sys_mass, ctrl_type, act_damp)
%% Load the controller and save it for the simulation
if strcmp(ctrl_type, 'cl') && strcmp(act_damp, 'none')
K_obj = load('./mat/K_fb.mat');
K = K_obj.(sprintf('K_%s_%s', sys_mass, sys_name)); %#ok
save('./mat/controller.mat', 'K');
elseif strcmp(ctrl_type, 'cl') && strcmp(act_damp, 'iff')
K_obj = load('./mat/K_fb_iff.mat');
K = K_obj.(sprintf('K_%s_%s_iff', sys_mass, sys_name)); %#ok
save('./mat/controller.mat', 'K');
elseif strcmp(ctrl_type, 'ol')
K = tf(zeros(6)); %#ok
save('./mat/controller.mat', 'K');
else
error('ctrl_type should be cl or ol');
end
%% Active Damping
if strcmp(act_damp, 'iff')
K_iff_crit = load('./mat/K_iff_crit.mat');
K_iff = K_iff_crit.(sprintf('K_iff_%s_%s', sys_mass, sys_name)); %#ok
save('./mat/K_iff.mat', 'K_iff');
elseif strcmp(act_damp, 'none')
K_iff = tf(zeros(6)); %#ok
save('./mat/K_iff.mat', 'K_iff');
end
%%
if strcmp(sys_name, 'pz')
initializeNanoHexapod(struct('actuator', 'piezo'));
elseif strcmp(sys_name, 'vc')
initializeNanoHexapod(struct('actuator', 'lorentz'));
else
error('sys_name should be pz or vc');
end
if strcmp(sys_mass, 'light')
initializeSample(struct('mass', 1));
elseif strcmp(sys_mass, 'heavy')
initializeSample(struct('mass', 50));
else
error('sys_mass should be light or heavy');
end
%% Run the simulation
sim('Assemblage.slx');
%% Split the Dsample matrix into vectors
[Dx, Dy, Dz, Rx, Ry, Rz] = matSplit(Dsample.Data, 1); %#ok
time = Dsample.Time; %#ok
%% Save the result
filename = sprintf('sim_%s_%s_%s_%s', sys_mass, sys_name, ctrl_type, act_damp);
save(sprintf('./mat/%s.mat', filename), ...
'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
end