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

change initialization scripts

Browse Source
This commit is contained in:
Thomas Dehaeze 2021-07-01 17:45:57 +02:00
parent 6cd48b77e9
commit 8281fd545a
4 changed files with 124 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
matlab/identif_save.m
View File

@ -1,34 +1,30 @@
% =frf_save.m= - Save Data
% :PROPERTIES:
% :header-args: :tangle matlab/frf_save.m
% :END:
% First, we get data from the Speedgoat:
tg = slrt;
f = SimulinkRealTime.openFTP(tg);
mget(f, 'data/data.dat');
close(f);
% 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); % Measured voltage (force sensor) [V]
u = data(:, 13:18); % DAC Voltage (command) [V]
Vexc = data(:, 19); % Excitation Voltage [V]
Vs = data(:, 7:12); % Force Sensor [V]
u = data(:, 13:18); % Control Output [V]
Va = data(:, 19); % 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');
% And we save this to a =mat= file:
strut_number = 6;
save('mat/hac_iff_more_lead_nass_scan', 't', 'de', 'Vs', 'u', 'Va');
save(sprintf('mat/iff_strut_%i_noise_g_400.mat', strut_number), 't', 'u', 'Vs', 'Vexc', 'de');
% save(sprintf('mat/frf_data_exc_strut_%i_noise_lf.mat', strut_number), 't', 'u', 'Vs', 'de');
% save(sprintf('mat/frf_data_exc_strut_%i_sweep.mat', strut_number), 't', 'u', 'Vs', 'de');
% save(sprintf('mat/iff_strut_%i_noise_hf.mat', strut_number), 't', 'u', 'Vs', 'de');
% save(sprintf('mat/frf_data_exc_strut_%i_add_mass_closed_circuit.mat', strut_number), 't', 'u', 'Vs', 'de');
%%
load('mat/jacobian.mat', 'J');
X = inv(J)*de';
X = X';
%%
figure;
plot3(X(:,1), X(:,2), X(:,3))

97
matlab/init_hac_iff.m
View File

@ -1,7 +1,96 @@
%% HAC
load('mat/Khac_iff_struts.mat', 'Khac_iff_struts');
save('sim_data/Khac_iff_struts.mat', 'Khac_iff_struts');
%%
clear;
%% Jacobian
load('mat/jacobian.mat', 'J');
save('sim_data/J.mat', 'J');
Jinv = pinv(J);
save('sim_data/J.mat', 'J', 'Jinv');
%% 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
%% Save Controller
load('sim_data/data_sim.mat', 'Ts')
Khac_iff_struts = c2d(Khac_iff_struts, 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')
L_frf = zeros(size(G_enc_iff_opt));
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');
end
colors = colororder;
freqs = 2*logspace(1, 3, 1000);
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
% Diagonal Elements FRF
plot(f, abs(L_frf(:,1,1)), 'color', colors(1,:), ...
'DisplayName', 'Diagonal');
for i = 2:6
plot(f, abs(L_frf(:,i,i)), 'color', colors(1,:), ...
'HandleVisibility', 'off');
end
plot(f, abs(L_frf(:,1,2)), 'color', [colors(2,:), 0.2], ...
'DisplayName', 'Off-Diag');
for i = 1:5
for j = i+1:6
plot(f, abs(L_frf(:,i,j)), 'color', [colors(2,:), 0.2], ...
'HandleVisibility', 'off');
end
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Loop Gain [-]'); set(gca, 'XTickLabel',[]);
ylim([1e-3, 1e2]);
legend('location', 'northeast');
grid;
ax2 = nexttile;
hold on;
for i =1:6
plot(f, 180/pi*angle(L_frf(:,i,i)), 'color', colors(1,:));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-180, 180]);
grid;
linkaxes([ax1,ax2],'x');
xlim([1, 2e3]);

7
matlab/init_iff.m
View File

@ -1,5 +1,12 @@
%%
clear;
%%
load('mat/Kiff.mat', 'Kiff');
%%
load('sim_data/data_sim.mat', 'Ts')
Kiff = c2d(Kiff, Ts, 'Tustin');
%%
save('sim_data/Kiff.mat', 'Kiff');

11
matlab/init_simulation.m
View File

@ -7,12 +7,12 @@ s = zpk('s');
addpath('./src/');
%% Simulation configuration
Fs = 5e3; % Sampling Frequency [Hz]
Fs = 10e3; % Sampling Frequency [Hz]
Ts = 1/Fs; % Sampling Time [s]
%% Data record configuration
Trec_start = 10; % Start time for Recording [s]
Trec_dur = 600; % Recording Duration [s]
Trec_start = 5; % Start time for Recording [s]
Trec_dur = 60; % Recording Duration [s]
Tsim = 2*Trec_start + Trec_dur; % Simulation Time [s]
@ -122,4 +122,7 @@ set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlim([1, Fs/2]); ylim([1e-10, 1e0]);
%% Save data that will be loaded in the Simulink file
save('sim_data/data_sim.mat', 'Fs', 'Ts', 'Tsim', 'Trec_start', 'Trec_dur', 'V_exc');
save('sim_data/data_sim.mat', ...
'Fs', 'Ts', 'Tsim', 'Trec_start', 'Trec_dur', ...
'V_exc', 'V_off', ...
'u_min', 'u_max');