tdehaeze/test-bench-nano-hexapod
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

update scripts and model

Browse Source
This commit is contained in:
Thomas Dehaeze 2021-08-11 18:03:35 +02:00
parent a988122f04
commit 8dfa7f351f
12 changed files with 212 additions and 98 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
matlab/identif_S_T.m Normal file
View File

@ -0,0 +1,24 @@
load('mat/T_S_meas_Dz_3m_hac_svd_iff.mat', 't', 'de', 'Vs', 'u', 'Va', 'Rx', 'Kiff', 'Khac_iff_struts');
Ts = (t(end) - t(1))/(length(t)-1);
Fs = 1/Ts;
win = hanning(ceil(10*Fs)); % Hannning Windows
load('mat/jacobian.mat', 'J');
y = (inv(J)*de')';
[T, f] = tfestimate(Rx(:,3), y(:,3), win, [], [], 1/Ts);
[S, ~] = tfestimate(Rx(:,3), Rx(:,3)-y(:,3), win, [], [], 1/Ts);
figure;
hold on;
plot(f, abs(T));
plot(f, abs(S));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude');
grid;
xlim([1, 2e3]);

11
matlab/identif_analyze.m
View File

@ -10,8 +10,8 @@ addpath('./src/');
%% Load measurement data for APA number 1
strut_number = 1;
% load(sprintf('mat/frf_data_exc_strut_%i_noise_lf.mat', strut_number), 't', 'Va', 'Vs', 'de');
load(sprintf('mat/iff_strut_%i_noise.mat', strut_number), 't', 'Va', 'Vs', 'de');
load(sprintf('mat/frf_data_exc_strut_%i_spindle_0m.mat', strut_number), 't', 'Va', 'Vs', 'de');
% load(sprintf('mat/iff_strut_%i_noise.mat', strut_number), 't', 'Va', 'Vs', 'de');
% Compute transfer functions:
@ -21,7 +21,7 @@ Fs = 1/Ts;
win = hanning(ceil(1*Fs)); % Hannning Windows
%% DVF
[G_dvf, f] = tfestimate(Vexc, de, win, [], [], 1/Ts);
[G_dvf, f] = tfestimate(Va, de, win, [], [], 1/Ts);
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
@ -29,12 +29,14 @@ tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
for i =1:6
plot(f, abs(G_dvf(:,i)));
plot(f, abs(G_dvf(:,i)), 'DisplayName', sprintf('Leg %i', i));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_{out}/V_{in}$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
legend;
grid;
ax2 = nexttile;
hold on;
@ -46,6 +48,7 @@ set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
grid;
linkaxes([ax1,ax2],'x');
xlim([5, 5e3]);

44
matlab/identif_analyze_all.m
View File

@ -9,42 +9,34 @@ addpath('./src/');
% Test with one APA
%% Load measurement data for APA number 1
meas_data_lf = {};
meas_data = {};
for i = 1:6
meas_data_lf(i) = {load(sprintf('mat/frf_data_exc_strut_%i_noise_lf.mat', i), 't', 'Va', 'Vs', 'de')};
meas_data_hf(i) = {load(sprintf('mat/frf_data_exc_strut_%i_noise_hf.mat', i), 't', 'Va', 'Vs', 'de')};
meas_data(i) = {load(sprintf('mat/frf_data_exc_strut_%i_spindle_0m.mat', i), 't', 'Va', 'Vs', 'de')};
end
%%
Ts = (meas_data_lf{1}.t(end) - (meas_data_lf{1}.t(1)))/(length(meas_data_lf{1}.t)-1);
Ts = (meas_data{1}.t(end) - (meas_data{1}.t(1)))/(length(meas_data{1}.t)-1);
Fs = 1/Ts;
win = hanning(ceil(1*Fs)); % Hannning Windows
%% DVF
[~, f] = tfestimate(meas_data_lf{1}.Va, meas_data_lf{1}.de, win, [], [], 1/Ts);
[~, f] = tfestimate(meas_data{1}.Va, meas_data{1}.de, win, [], [], 1/Ts);
G_dvf_lf = zeros(length(f), 6, 6);
G_dvf_hf = zeros(length(f), 6, 6);
G_dvf = zeros(length(f), 6, 6);
for i = 1:6
G_dvf_lf(:, :, i) = tfestimate(meas_data_lf{i}.Va, meas_data_lf{i}.de, win, [], [], 1/Ts);
G_dvf_hf(:, :, i) = tfestimate(meas_data_hf{i}.Va, meas_data_hf{i}.de, win, [], [], 1/Ts);
G_dvf(:, :, i) = tfestimate(meas_data{i}.Va, meas_data{i}.de, win, [], [], 1/Ts);
end
%% IFF
[~, f] = tfestimate(meas_data_lf{1}.Va, meas_data_lf{1}.de, win, [], [], 1/Ts);
[~, f] = tfestimate(meas_data{1}.Va, meas_data{1}.de, win, [], [], 1/Ts);
G_iff_lf = zeros(length(f), 6, 6);
G_iff_hf = zeros(length(f), 6, 6);
G_iff = zeros(length(f), 6, 6);
for i = 1:6
G_iff_lf(:, :, i) = tfestimate(meas_data_lf{i}.Va, meas_data_lf{i}.Vs, win, [], [], 1/Ts);
G_iff_hf(:, :, i) = tfestimate(meas_data_hf{i}.Va, meas_data_hf{i}.Vs, win, [], [], 1/Ts);
G_iff(:, :, i) = tfestimate(meas_data{i}.Va, meas_data{i}.Vs, win, [], [], 1/Ts);
end
%%
i_lf = f < 250;
i_hf = f > 250;
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
@ -52,9 +44,7 @@ ax1 = nexttile([2,1]);
hold on;
for i =1:6
set(gca,'ColorOrderIndex',i)
plot(f(i_lf), abs(G_dvf_lf(i_lf,i, i)));
set(gca,'ColorOrderIndex',i)
plot(f(i_hf), abs(G_dvf_hf(i_hf,i, i)));
plot(f, abs(G_dvf(:,i, i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
@ -65,9 +55,7 @@ ax2 = nexttile;
hold on;
for i =1:6
set(gca,'ColorOrderIndex',i)
plot(f(i_lf), 180/pi*angle(G_dvf_lf(i_lf,i, i)));
set(gca,'ColorOrderIndex',i)
plot(f(i_hf), 180/pi*angle(G_dvf_hf(i_hf,i, i)));
plot(f, 180/pi*angle(G_dvf(:,i, i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
@ -76,7 +64,7 @@ hold off;
yticks(-360:90:360);
linkaxes([ax1,ax2],'x');
xlim([20, 2e3]);
xlim([10, 1e3]);
%%
figure;
@ -86,9 +74,7 @@ ax1 = nexttile([2,1]);
hold on;
for i =1:6
set(gca,'ColorOrderIndex',i)
plot(f(i_lf), abs(G_iff_lf(i_lf,i, i)));
set(gca,'ColorOrderIndex',i)
plot(f(i_hf), abs(G_iff_hf(i_hf,i, i)));
plot(f, abs(G_iff(:,i, i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
@ -99,9 +85,7 @@ ax2 = nexttile;
hold on;
for i =1:6
set(gca,'ColorOrderIndex',i)
plot(f(i_lf), 180/pi*angle(G_iff_lf(i_lf,i, i)));
set(gca,'ColorOrderIndex',i)
plot(f(i_hf), 180/pi*angle(G_iff_hf(i_hf,i, i)));
plot(f, 180/pi*angle(G_iff(:,i, i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');

29
matlab/identif_save.m
View File

@ -1,30 +1,35 @@
%%
tg = slrt;
f = SimulinkRealTime.openFTP(tg);
mget(f, 'data/data.dat');
close(f);
% And we load the data on the Workspace:
%% And we load the data on the Workspace:
data = SimulinkRealTime.utils.getFileScopeData('data/data.dat').data;
de = data(:, 1:6); % Measurment displacement (encoder) [m]
Vs = data(:, 7:12); % Force Sensor [V]
u = data(:, 13:18); % Control Output [V]
Va = data(:, 19); % Excitation Signal [V]
Rx = data(:, 20:25); % Excitation Signal [V]
t = data(:, end); % Time [s]
% strut_num = 6;
% save(sprintf('mat/frf_exc_iff_strut_%i_enc_plates_noise.mat', strut_num), 't', 'de', 'Vs', 'u', 'Va');
save('mat/hac_iff_more_lead_nass_scan', 't', 'de', 'Vs', 'u', 'Va');
%%
load('sim_data/Khac_iff_struts.mat', 'Khac_iff_struts');
load('sim_data/Kiff.mat', 'Kiff');
%%
load('mat/jacobian.mat', 'J');
save('mat/T_S_meas_Rz_3m_hac_svd_iff.mat', 't', 'de', 'Vs', 'u', 'Va', 'Rx', 'Kiff', 'Khac_iff_struts');
X = inv(J)*de';
X = X';
% save('mat/hac_iff_more_lead_nass_scan', 't', 'de', 'Vs', 'u', 'Va');
%%
figure;
plot3(X(:,1), X(:,2), X(:,3))
% %%
% load('mat/jacobian.mat', 'J');
%
% X = inv(J)*de';
% X = X';
%
% %%
% figure;
% plot3(X(:,1), X(:,2), X(:,3))

BIN
matlab/iff_measure.slx
View File

Binary file not shown.

44
matlab/init_hac_iff.m
View File

@ -1,51 +1,25 @@
%%
clear;
%% Jacobian
load('mat/jacobian.mat', 'J');
Jinv = pinv(J);
save('sim_data/J.mat', 'J', 'Jinv');
clear; close all; clc;
%% Saved HAC
% load('mat/Khac_iff_struts.mat', 'Khac_iff_struts');
%% Developed HAC
s = zpk('s');
a = 5; % Amount of phase lead / width of the phase lead / high frequency gain
wc = 2*pi*110; % Frequency with the maximum phase lead [rad/s]
H_lead = (1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
H_lpf = 1/(1 + s/2/pi/300);
gm = 0.02;
xi = 0.5;
wn = 2*pi*700;
H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
Khac_iff_struts = -2.2e4 * ... % Gain
H_lead * ... % Lead
H_lpf * ... % Lead
H_notch * ... % Notch
(2*pi*100/s) * ... % Integrator
eye(6); % 6x6 Diagonal
% load('mat/Khac_iff_struts_rob.mat', 'Kd');
load('mat/Khac_iff_struts_svd.mat', 'Kd');
% load('mat/Khac_iff_struts_jacobian_cok.mat', 'Kd');
%% Save Controller
load('sim_data/data_sim.mat', 'Ts')
Khac_iff_struts = c2d(Khac_iff_struts, Ts, 'Tustin');
Khac_iff_struts = c2d(Kd, Ts, 'Tustin');
save('sim_data/Khac_iff_struts.mat', 'Khac_iff_struts');
%% Loop Gain
load('mat/damped_plant_enc_plates.mat', 'f', 'G_enc_iff_opt')
load('mat/frf_iff_vib_table_m.mat', 'f', 'G_dL');
L_frf = zeros(size(G_enc_iff_opt));
L_frf = zeros(size(G_dL{3}));
for i = 1:size(G_enc_iff_opt, 1)
L_frf(i, :, :) = squeeze(G_enc_iff_opt(i,:,:))*freqresp(Khac_iff_struts, f(i), 'Hz');
for i = 1:length(f)
L_frf(i, :, :) = squeeze(G_dL{3}(i,:,:))*freqresp(Khac_iff_struts, f(i), 'Hz');
end
colors = colororder;

10
matlab/init_iff.m
View File

@ -1,8 +1,14 @@
%%
clear;
clear; close all; clc;
%%
load('mat/Kiff.mat', 'Kiff');
% load('mat/Kiff.mat', 'Kiff');
s = zpk( 's');
Kiff = -2e2*...
(1/(s + 2*pi*20))*... % LPF: provides integral action above x[Hz]
(s/(s + 2*pi*20))*... % HPF: limit low frequency gain
(1/(1 + s/2/pi/400))*... % LPF: more robust to high frequency resonances
eye(6); % Diagonal controller
%%
load('sim_data/data_sim.mat', 'Ts')

38
matlab/init_ref_dist.m Normal file
View File

@ -0,0 +1,38 @@
%%
clear; close all; clc;
%%
load('sim_data/data_sim.mat', 'Trec_start', 'Trec_dur', 'Ts');
%% White Noise => Identification of the sensibility transfer function
s = zpk('s');
Rx_noise = generateShapedNoise('Ts', Ts, ...
'V_mean', 0, ...
't_start', Trec_start, ...
'exc_duration', Trec_dur, ...
'V_exc', 2e-6/(2*pi*0.2 + s));
R_dist = timeseries(Rx_noise(2, :), Rx_noise(1, :));
figure;
tiledlayout(1, 2, 'TileSpacing', 'Normal', 'Padding', 'None');
ax1 = nexttile;
hold on;
plot(R_dist.Time, squeeze(R_dist.Data(1, 1, :)));
xlabel('Time [s]'); ylabel('Amplitude [m,rad]');
ax2 = nexttile;
hold on;
win = hanning(floor(length(squeeze(R_dist.Data(1, 1, :)))/8));
[pxx, f] = pwelch(squeeze(R_dist.Data(1, 1, :)), win, 0, [], 1/Ts);
plot(f, sqrt(pxx))
hold off;
xlabel('Frequency [Hz]'); ylabel('Amplitude Spectral Density [$m/\sqrt{Hz}$]');
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlim([1, 1e3]); ylim([1e-10, 1e0]);
%%
save('sim_data/ref_dist.mat', 'R_dist');

44
matlab/init_simulation.m
View File

@ -7,18 +7,24 @@ s = zpk('s');
addpath('./src/');
%% Simulation configuration
Fs = 10e3; % Sampling Frequency [Hz]
Fs = 1e4; % Sampling Frequency [Hz]
Ts = 1/Fs; % Sampling Time [s]
%% Data record configuration
Trec_start = 5; % Start time for Recording [s]
Trec_dur = 60; % Recording Duration [s]
Trec_dur = 100; % Recording Duration [s]
Tsim = 2*Trec_start + Trec_dur; % Simulation Time [s]
%% Security
u_min = -1;
u_max = 7.5;
% u_min = -1;
% u_max = 7.5;
u_min = 1.25;
u_max = 5.25;
d_min = -50e-6;
d_max = 50e-6;
%% Shaped Noise
V_noise = generateShapedNoise('Ts', 1/Fs, ...
@ -46,12 +52,12 @@ V_sweep = generateSweepExc('Ts', Ts, ...
'V_exc', G_sweep*1/(1 + s/2/pi/500));
V_sweep_lf = generateSweepExc('Ts', Ts, ...
'f_start', 0.1, ...
'f_end', 10, ...
'f_start', 20, ...
'f_end', 40, ...
'V_mean', 0, ...
't_start', Trec_start, ...
'exc_duration', Trec_dur, ...
'sweep_type', 'log', ...
'sweep_type', 'lin', ...
'V_exc', 0.2);
%% High Frequency Shaped Noise
@ -66,8 +72,8 @@ V_noise_hf = generateShapedNoise('Ts', Ts, ...
'V_exc', wL);
%% Low Frequency Shaped Noise
[b,a] = cheby1(10, 2, 2*pi*[10 260], 'bandpass', 's');
wL = 0.005*tf(b, a);
[b,a] = cheby1(10, 2, 2*pi*[20 40], 'bandpass', 's');
wL = 0.01*tf(b, a);
V_noise_lf = generateShapedNoise('Ts', 1/Fs, ...
'V_mean', 0, ...
@ -76,6 +82,17 @@ V_noise_lf = generateShapedNoise('Ts', 1/Fs, ...
'smooth_ends', true, ...
'V_exc', wL);
%% Band Pass Shaped Noise
[b,a] = cheby1(10, 2, 2*pi*[9 1.1e3], 'bandpass', 's');
wL = 0.005*tf(b, a);
V_noise_bf = generateShapedNoise('Ts', 1/Fs, ...
'V_mean', 0, ...
't_start', Trec_start, ...
'exc_duration', Trec_dur, ...
'smooth_ends', true, ...
'V_exc', wL);
%% Sinus excitation with increasing amplitude
V_sin = generateSinIncreasingAmpl('Ts', 1/Fs, ...
'V_mean', 0, ...
@ -102,7 +119,7 @@ V_zero_off = generateShapedNoise('Ts', 1/Fs, ...
'V_exc', tf(0));
%% Select the excitation signal and offset voltage
V_exc = timeseries(V_zero(2,:), V_zero(1,:));
V_exc = timeseries(V_noise_bf(2,:), V_noise_bf(1,:));
V_off = timeseries(V_zero_off(2,:), V_zero_off(1,:));
%% Plot
@ -110,7 +127,11 @@ figure;
tiledlayout(1, 2, 'TileSpacing', 'Normal', 'Padding', 'None');
ax1 = nexttile;
hold on;
plot(V_exc.Time, squeeze(V_exc.Data+V_off.Data));
plot([V_exc.Time(1), V_exc.Time(end)], [u_min, u_min], 'k--');
plot([V_exc.Time(1), V_exc.Time(end)], [u_max, u_max], 'k--');
hold off;
xlabel('Time [s]'); ylabel('Amplitude [V]');
ax2 = nexttile;
@ -125,4 +146,5 @@ xlim([1, Fs/2]); ylim([1e-10, 1e0]);
save('sim_data/data_sim.mat', ...
'Fs', 'Ts', 'Tsim', 'Trec_start', 'Trec_dur', ...
'V_exc', 'V_off', ...
'u_min', 'u_max');
'u_min', 'u_max', ...
'd_min', 'd_max');

54
matlab/run_batch_id_S_T.m Normal file
View File

@ -0,0 +1,54 @@
%% First make sure the model is open, and we are connected to the
% Speedgoat Machine.
labels = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
%% Run Multiple Simulations
my_model = 'iff_measure';
tg = slrt;
%% For each strut
for i_dir = 1:6
%% Get excitation strut
p = Simulink.Mask.get([my_model, '/Subsystem5']);
p.Parameters.Value = sprintf('%i', i_dir);
%% Connect
set_param(my_model,'SimulationCommand','connect')
%% Run the simulation
sprintf('Start measurement in %s', labels{i_dir})
set_param(my_model,'SimulationCommand','start')
%% Wait for the simulation to finish
pause(1)
while strcmp(get_param(my_model,'SimulationStatus'), 'external')
pause(1)
end
sprintf('Finished excitation for %s', labels{i_dir})
%% Disconnect
set_param(my_model,'SimulationCommand','disconnect')
%% Save the data
f = SimulinkRealTime.openFTP(tg);
mget(f, 'data/data.dat');
close(f);
data = SimulinkRealTime.utils.getFileScopeData('data/data.dat').data;
de = data(:, 1:6); % Measurment displacement (encoder) [m]
Vs = data(:, 7:12); % Force Sensor [V]
u = data(:, 13:18); % Control Output [V]
Va = data(:, 19); % Excitation Signal [V]
Rx = data(:, 20:25); % Reference Signal [m/rad]
t = data(:, end); % Time [s]
load('sim_data/data_sim.mat', 'Ts'); % To save Sampling Period
load('sim_data/Kiff.mat', 'Kiff'); % To save Controller
load('sim_data/Khac_iff_struts.mat', 'Khac_iff_struts'); % To save Controller
save(sprintf('mat/T_S_meas_%s_2m_hac_svd_iff.mat', labels{i_dir}), ...
't', 'Ts', 'de', 'Vs', 'u', 'Va', 'Kiff', 'Khac_iff_struts');
sprintf('Saved Data for strut %s', labels{i_dir})
end

12
matlab/run_batch_identification.m
View File

@ -10,9 +10,11 @@ for i_leg = 1:6
%% Get excitation strut
p = Simulink.Mask.get([my_model, '/Subsystem7']);
p.Parameters.Value = sprintf('%i', i_leg);
pause(0.1);
%% Connect
set_param(my_model,'SimulationCommand','connect')
set_param(my_model,'SimulationCommand','connect');
pause(0.1);
%% Run the simulation
sprintf('Start excitation for strut %i', i_leg)
@ -23,10 +25,11 @@ for i_leg = 1:6
while strcmp(get_param(my_model,'SimulationStatus'), 'external')
pause(1)
end
sprintf('Finished excitation for strut %i', i_leg)
sprintf('Finished excitation for strut %i', i_leg);
%% Disconnect
set_param(my_model,'SimulationCommand','disconnect')
set_param(my_model,'SimulationCommand','disconnect');
pause(0.1);
%% Save the data
f = SimulinkRealTime.openFTP(tg);
@ -44,8 +47,9 @@ for i_leg = 1:6
load('sim_data/data_sim.mat', 'Ts'); % To save Sampling Period
save('sim_data/Kiff.mat', 'Kiff'); % To save Controller
save(sprintf('mat/frf_data_exc_strut_%i_iff_vib_table_0m.mat', i_leg), ...
save(sprintf('mat/frf_data_exc_strut_%i_spindle_3m_iff_60rpm.mat', i_leg), ...
't', 'Ts', 'de', 'Vs', 'u', 'Va', 'Kiff');
sprintf('Saved Data for strut %i', i_leg)
pause(0.1);
end

BIN
matlab/test_spindle.slx
View File

Binary file not shown.