change initialization scripts
This commit is contained in:
parent
6cd48b77e9
commit
8281fd545a
@ -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]
|
||||
t = data(:, end); % Time [s]
|
||||
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]
|
||||
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))
|
@ -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]);
|
||||
|
@ -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');
|
@ -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');
|
||||
|
Loading…
Reference in New Issue
Block a user