Initial Commit
This commit is contained in:
Binary file not shown.
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
+112951
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because it is too large
Load Diff
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because it is too large
Load Diff
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because it is too large
Load Diff
File diff suppressed because one or more lines are too long
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,225 @@
|
||||
% Matlab Init :noexport:ignore:
|
||||
|
||||
%% dcm_active_damping_iff.m
|
||||
% Test of Integral Force Feedback Strategy
|
||||
|
||||
%% Clear Workspace and Close figures
|
||||
clear; close all; clc;
|
||||
|
||||
%% Intialize Laplace variable
|
||||
s = zpk('s');
|
||||
|
||||
%% Path for functions, data and scripts
|
||||
addpath('./mat/'); % Path for data
|
||||
|
||||
%% Simscape Model - Nano Hexapod
|
||||
addpath('./STEPS/')
|
||||
|
||||
%% Initialize Parameters for Simscape model
|
||||
controller.type = 0; % Open Loop Control
|
||||
|
||||
%% Options for Linearization
|
||||
options = linearizeOptions;
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Open Simulink Model
|
||||
mdl = 'simscape_dcm';
|
||||
|
||||
open(mdl)
|
||||
|
||||
%% Colors for the figures
|
||||
colors = colororder;
|
||||
|
||||
%% Frequency Vector
|
||||
freqs = logspace(1, 3, 1000);
|
||||
|
||||
% Identification
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
|
||||
%% Inputs
|
||||
% Control Input {3x1} [N]
|
||||
io(io_i) = linio([mdl, '/control_system'], 1, 'openinput'); io_i = io_i + 1;
|
||||
|
||||
%% Outputs
|
||||
% Force Sensor {3x1} [m]
|
||||
io(io_i) = linio([mdl, '/DCM'], 3, 'openoutput'); io_i = io_i + 1;
|
||||
|
||||
%% Extraction of the dynamics
|
||||
G_fs = linearize(mdl, io);
|
||||
|
||||
G_fs.InputName = {'u_ur', 'u_uh', 'u_d'};
|
||||
G_fs.OutputName = {'fs_ur', 'fs_uh', 'fs_d'};
|
||||
|
||||
|
||||
|
||||
% #+RESULTS:
|
||||
% | -1.4113e-13 | 1.0339e-13 | 3.774e-14 |
|
||||
% | 1.0339e-13 | -1.4113e-13 | 3.774e-14 |
|
||||
% | 3.7792e-14 | 3.7792e-14 | -7.5585e-14 |
|
||||
|
||||
|
||||
%% Bode plot for the plant
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_fs(1,1), freqs, 'Hz'))), ...
|
||||
'DisplayName', 'd');
|
||||
plot(freqs, abs(squeeze(freqresp(G_fs(2,2), freqs, 'Hz'))), ...
|
||||
'DisplayName', 'uh');
|
||||
plot(freqs, abs(squeeze(freqresp(G_fs(3,3), freqs, 'Hz'))), ...
|
||||
'DisplayName', 'ur');
|
||||
plot(freqs, abs(squeeze(freqresp(G_fs(1,2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
||||
'DisplayName', 'off-diag');
|
||||
for i = 1:2
|
||||
for j = i+1:3
|
||||
plot(freqs, abs(squeeze(freqresp(G_fs(i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 2);
|
||||
ylim([1e-13, 1e-7]);
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_fs(1,1), freqs, 'Hz'))));
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_fs(2,2), freqs, 'Hz'))));
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_fs(3,3), freqs, 'Hz'))));
|
||||
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]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
|
||||
% Controller - Root Locus
|
||||
|
||||
Kiff_g1 = eye(3)*1/(1 + s/2/pi/20);
|
||||
|
||||
%% Root Locus for IFF
|
||||
gains = logspace(9, 12, 200);
|
||||
|
||||
figure;
|
||||
|
||||
hold on;
|
||||
plot(real(pole(G_fs)), imag(pole(G_fs)), 'x', 'color', colors(1,:), ...
|
||||
'DisplayName', '$g = 0$');
|
||||
plot(real(tzero(G_fs)), imag(tzero(G_fs)), 'o', 'color', colors(1,:), ...
|
||||
'HandleVisibility', 'off');
|
||||
|
||||
for g = gains
|
||||
clpoles = pole(feedback(G_fs, g*Kiff_g1, +1));
|
||||
plot(real(clpoles), imag(clpoles), '.', 'color', colors(1,:), ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
|
||||
% Optimal gain
|
||||
g = 8e10;
|
||||
clpoles = pole(feedback(G_fs, g*Kiff_g1, +1));
|
||||
plot(real(clpoles), imag(clpoles), 'x', 'color', colors(2,:), ...
|
||||
'DisplayName', sprintf('$g=%.0e$', g));
|
||||
hold off;
|
||||
axis square;
|
||||
xlim([-2700, 0]); ylim([0, 2700]);
|
||||
xlabel('Real Part'); ylabel('Imaginary Part');
|
||||
legend('location', 'northwest');
|
||||
|
||||
|
||||
|
||||
% #+name: fig:iff_root_locus
|
||||
% #+caption: Root Locus plot for the IFF Control strategy
|
||||
% #+RESULTS:
|
||||
% [[file:figs/iff_root_locus.png]]
|
||||
|
||||
|
||||
%% Integral Force Feedback Controller
|
||||
Kiff = g*Kiff_g1;
|
||||
|
||||
% Damped Plant
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
|
||||
%% Inputs
|
||||
% Control Input {3x1} [N]
|
||||
io(io_i) = linio([mdl, '/control_system'], 1, 'input'); io_i = io_i + 1;
|
||||
|
||||
%% Outputs
|
||||
% Force Sensor {3x1} [m]
|
||||
io(io_i) = linio([mdl, '/DCM'], 1, 'openoutput'); io_i = io_i + 1;
|
||||
|
||||
%% DCM Kinematics
|
||||
load('mat/dcm_kinematics.mat');
|
||||
|
||||
%% Identification of the Open Loop plant
|
||||
controller.type = 0; % Open Loop
|
||||
G_ol = J_a_111*inv(J_s_111)*linearize(mdl, io);
|
||||
G_ol.InputName = {'u_ur', 'u_uh', 'u_d'};
|
||||
G_ol.OutputName = {'d_ur', 'd_uh', 'd_d'};
|
||||
|
||||
%% Identification of the damped plant with IFF
|
||||
controller.type = 1; % IFF
|
||||
G_dp = J_a_111*inv(J_s_111)*linearize(mdl, io);
|
||||
G_dp.InputName = {'u_ur', 'u_uh', 'u_d'};
|
||||
G_dp.OutputName = {'d_ur', 'd_uh', 'd_d'};
|
||||
|
||||
%% Comparison of the damped and undamped plant
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_ol(1,1), freqs, 'Hz'))), ...
|
||||
'DisplayName', 'd - OL');
|
||||
plot(freqs, abs(squeeze(freqresp(G_ol(2,2), freqs, 'Hz'))), ...
|
||||
'DisplayName', 'uh - OL');
|
||||
plot(freqs, abs(squeeze(freqresp(G_ol(3,3), freqs, 'Hz'))), ...
|
||||
'DisplayName', 'ur - OL');
|
||||
set(gca,'ColorOrderIndex',1)
|
||||
plot(freqs, abs(squeeze(freqresp(G_dp(1,1), freqs, 'Hz'))), '--', ...
|
||||
'DisplayName', 'd - IFF');
|
||||
plot(freqs, abs(squeeze(freqresp(G_dp(2,2), freqs, 'Hz'))), '--', ...
|
||||
'DisplayName', 'uh - IFF');
|
||||
plot(freqs, abs(squeeze(freqresp(G_dp(3,3), freqs, 'Hz'))), '--', ...
|
||||
'DisplayName', 'ur - IFF');
|
||||
for i = 1:2
|
||||
for j = i+1:3
|
||||
plot(freqs, abs(squeeze(freqresp(G_dp(i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
|
||||
ylim([1e-12, 1e-6]);
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ol(1,1), freqs, 'Hz'))));
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ol(2,2), freqs, 'Hz'))));
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ol(3,3), freqs, 'Hz'))));
|
||||
set(gca,'ColorOrderIndex',1)
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dp(1,1), freqs, 'Hz'))), '--');
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dp(2,2), freqs, 'Hz'))), '--');
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dp(3,3), freqs, 'Hz'))), '--');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||||
hold off;
|
||||
yticks(-360:90:360);
|
||||
ylim([-180, 0]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
|
||||
save('mat/Kiff.mat', 'Kiff');
|
||||
@@ -0,0 +1,101 @@
|
||||
% Matlab Init :noexport:ignore:
|
||||
|
||||
%% dcm_active_damping_strain_gauges.m
|
||||
% Active Damping using relative motion sensors (strain gauges)
|
||||
|
||||
%% Clear Workspace and Close figures
|
||||
clear; close all; clc;
|
||||
|
||||
%% Intialize Laplace variable
|
||||
s = zpk('s');
|
||||
|
||||
%% Path for functions, data and scripts
|
||||
addpath('./mat/'); % Path for data
|
||||
|
||||
%% Simscape Model - Nano Hexapod
|
||||
addpath('./STEPS/')
|
||||
|
||||
%% Initialize Parameters for Simscape model
|
||||
controller.type = 0; % Open Loop Control
|
||||
|
||||
%% Options for Linearization
|
||||
options = linearizeOptions;
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Open Simulink Model
|
||||
mdl = 'simscape_dcm';
|
||||
|
||||
open(mdl)
|
||||
|
||||
%% Colors for the figures
|
||||
colors = colororder;
|
||||
|
||||
%% Frequency Vector
|
||||
freqs = logspace(1, 3, 1000);
|
||||
|
||||
% Identification
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
|
||||
%% Inputs
|
||||
% Control Input {3x1} [N]
|
||||
io(io_i) = linio([mdl, '/u'], 1, 'openinput'); io_i = io_i + 1;
|
||||
% % Stepper Displacement {3x1} [m]
|
||||
% io(io_i) = linio([mdl, '/d'], 1, 'openinput'); io_i = io_i + 1;
|
||||
|
||||
%% Outputs
|
||||
% Strain Gauges {3x1} [m]
|
||||
io(io_i) = linio([mdl, '/sg'], 1, 'openoutput'); io_i = io_i + 1;
|
||||
|
||||
%% Extraction of the dynamics
|
||||
G_sg = linearize(mdl, io);
|
||||
|
||||
G_sg.InputName = {'u_ur', 'u_uh', 'u_d'};
|
||||
G_sg.OutputName = {'sg_ur', 'sg_uh', 'sg_d'};
|
||||
|
||||
|
||||
|
||||
% #+RESULTS:
|
||||
% | -1.4113e-13 | 1.0339e-13 | 3.774e-14 |
|
||||
% | 1.0339e-13 | -1.4113e-13 | 3.774e-14 |
|
||||
% | 3.7792e-14 | 3.7792e-14 | -7.5585e-14 |
|
||||
|
||||
|
||||
%% Bode plot for the plant
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_sg(1,1), freqs, 'Hz'))), ...
|
||||
'DisplayName', 'd');
|
||||
plot(freqs, abs(squeeze(freqresp(G_sg(2,2), freqs, 'Hz'))), ...
|
||||
'DisplayName', 'uh');
|
||||
plot(freqs, abs(squeeze(freqresp(G_sg(3,3), freqs, 'Hz'))), ...
|
||||
'DisplayName', 'ur');
|
||||
for i = 1:2
|
||||
for j = i+1:3
|
||||
plot(freqs, abs(squeeze(freqresp(G_sg(i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_sg(1,1), freqs, 'Hz'))));
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_sg(2,2), freqs, 'Hz'))));
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_sg(3,3), freqs, 'Hz'))));
|
||||
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]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
@@ -0,0 +1,34 @@
|
||||
% Matlab Init :noexport:ignore:
|
||||
|
||||
%% dcm_hac_iff.m
|
||||
% Development of the HAC-IFF control strategy
|
||||
|
||||
%% Clear Workspace and Close figures
|
||||
clear; close all; clc;
|
||||
|
||||
%% Intialize Laplace variable
|
||||
s = zpk('s');
|
||||
|
||||
%% Path for functions, data and scripts
|
||||
addpath('./mat/'); % Path for data
|
||||
|
||||
%% Simscape Model - Nano Hexapod
|
||||
addpath('./STEPS/')
|
||||
|
||||
%% Initialize Parameters for Simscape model
|
||||
controller.type = 0; % Open Loop Control
|
||||
|
||||
%% Options for Linearization
|
||||
options = linearizeOptions;
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Open Simulink Model
|
||||
mdl = 'simscape_dcm';
|
||||
|
||||
open(mdl)
|
||||
|
||||
%% Colors for the figures
|
||||
colors = colororder;
|
||||
|
||||
%% Frequency Vector
|
||||
freqs = logspace(1, 3, 1000);
|
||||
@@ -0,0 +1,202 @@
|
||||
% Matlab Init :noexport:ignore:
|
||||
|
||||
%% dcm_identification.m
|
||||
% Extraction of system dynamics using Simscape model
|
||||
|
||||
%% Clear Workspace and Close figures
|
||||
clear; close all; clc;
|
||||
|
||||
%% Intialize Laplace variable
|
||||
s = zpk('s');
|
||||
|
||||
%% Path for functions, data and scripts
|
||||
addpath('./mat/'); % Path for data
|
||||
|
||||
%% Simscape Model - Nano Hexapod
|
||||
addpath('./STEPS/')
|
||||
|
||||
%% Initialize Parameters for Simscape model
|
||||
controller.type = 0; % Open Loop Control
|
||||
|
||||
%% Options for Linearization
|
||||
options = linearizeOptions;
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Open Simulink Model
|
||||
mdl = 'simscape_dcm';
|
||||
|
||||
open(mdl)
|
||||
|
||||
%% Colors for the figures
|
||||
colors = colororder;
|
||||
|
||||
%% Frequency Vector
|
||||
freqs = logspace(1, 3, 1000);
|
||||
|
||||
|
||||
|
||||
% #+name: fig:schematic_system_inputs_outputs
|
||||
% #+caption: Dynamical system with inputs and outputs
|
||||
% #+RESULTS:
|
||||
% [[file:figs/schematic_system_inputs_outputs.png]]
|
||||
|
||||
% The system is identified from the Simscape model.
|
||||
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
|
||||
%% Inputs
|
||||
% Control Input {3x1} [N]
|
||||
io(io_i) = linio([mdl, '/control_system'], 1, 'openinput'); io_i = io_i + 1;
|
||||
|
||||
%% Outputs
|
||||
% Interferometers {3x1} [m]
|
||||
io(io_i) = linio([mdl, '/DCM'], 1, 'openoutput'); io_i = io_i + 1;
|
||||
|
||||
%% Extraction of the dynamics
|
||||
G = linearize(mdl, io);
|
||||
|
||||
%% Input and Output names
|
||||
G.InputName = {'u_ur', 'u_uh', 'u_d'};
|
||||
G.OutputName = {'int_111_1', 'int_111_2', 'int_111_3'};
|
||||
|
||||
% Plant in the frame of the fastjacks
|
||||
|
||||
load('mat/dcm_kinematics.mat');
|
||||
|
||||
|
||||
|
||||
% #+name: fig:schematic_jacobian_frame_fastjack
|
||||
% #+caption: Use of Jacobian matrices to obtain the system in the frame of the fastjacks
|
||||
% #+RESULTS:
|
||||
% [[file:figs/schematic_jacobian_frame_fastjack.png]]
|
||||
|
||||
|
||||
|
||||
%% Compute the system in the frame of the fastjacks
|
||||
G_pz = J_a_111*inv(J_s_111)*G;
|
||||
|
||||
|
||||
|
||||
% #+name: tab:dc_gain_plan_fj
|
||||
% #+caption: DC gain of the plant in the frame of the fast jacks $\bm{G}_{\text{fj}}$
|
||||
% #+attr_latex: :environment tabularx :width 0.5\linewidth :align ccc
|
||||
% #+attr_latex: :center t :booktabs t
|
||||
% #+RESULTS:
|
||||
% | 4.4407e-09 | 2.7656e-12 | 1.0132e-12 |
|
||||
% | 2.7656e-12 | 4.4407e-09 | 1.0132e-12 |
|
||||
% | 1.0109e-12 | 1.0109e-12 | 4.4424e-09 |
|
||||
|
||||
% The bode plot of $\bm{G}_{\text{fj}}(s)$ is shown in Figure [[fig:bode_plot_plant_fj]].
|
||||
|
||||
|
||||
%% Bode plot for the plant
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz(1,1), freqs, 'Hz'))), ...
|
||||
'DisplayName', 'd');
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz(2,2), freqs, 'Hz'))), ...
|
||||
'DisplayName', 'uh');
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz(3,3), freqs, 'Hz'))), ...
|
||||
'DisplayName', 'ur');
|
||||
for i = 1:2
|
||||
for j = i+1:3
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz(i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
||||
ylim([1e-13, 1e-6]);
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_pz(1,1), freqs, 'Hz'))));
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_pz(2,2), freqs, 'Hz'))));
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_pz(3,3), freqs, 'Hz'))));
|
||||
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]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
|
||||
|
||||
|
||||
% #+name: fig:schematic_jacobian_frame_crystal
|
||||
% #+caption: Use of Jacobian matrices to obtain the system in the frame of the crystal
|
||||
% #+RESULTS:
|
||||
% [[file:figs/schematic_jacobian_frame_crystal.png]]
|
||||
|
||||
|
||||
G_mr = inv(J_s_111)*G*inv(J_a_111');
|
||||
|
||||
|
||||
|
||||
% #+RESULTS:
|
||||
% | 1.9978e-09 | 3.9657e-09 | 7.7944e-09 |
|
||||
% | 3.9656e-09 | 8.4979e-08 | -1.5135e-17 |
|
||||
% | 7.7944e-09 | -3.9252e-17 | 1.834e-07 |
|
||||
|
||||
|
||||
%% Bode plot for the plant
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_mr(1,1), freqs, 'Hz'))), ...
|
||||
'DisplayName', 'd');
|
||||
plot(freqs, abs(squeeze(freqresp(G_mr(2,2), freqs, 'Hz'))), ...
|
||||
'DisplayName', 'uh');
|
||||
plot(freqs, abs(squeeze(freqresp(G_mr(3,3), freqs, 'Hz'))), ...
|
||||
'DisplayName', 'ur');
|
||||
for i = 1:2
|
||||
for j = i+1:3
|
||||
plot(freqs, abs(squeeze(freqresp(G_mr(i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_mr(1,1), freqs, 'Hz'))));
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_mr(2,2), freqs, 'Hz'))));
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_mr(3,3), freqs, 'Hz'))));
|
||||
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]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
|
||||
%% Bode plot for the plant
|
||||
fig = figure;
|
||||
tiledlayout(3, 3, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
for i_out = 1:3
|
||||
for i_in = 1:3
|
||||
ax = nexttile;
|
||||
plot(freqs, abs(squeeze(freqresp(G_mr(i_out, i_in), freqs, 'Hz'))));
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
end
|
||||
end
|
||||
|
||||
linkaxes(findall(fig, 'type', 'axes'),'xy');
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
@@ -0,0 +1,98 @@
|
||||
% Matlab Init :noexport:ignore:
|
||||
|
||||
%% dcm_kinematics.m
|
||||
% Computation of the DCM kinematics
|
||||
|
||||
%% Clear Workspace and Close figures
|
||||
clear; close all; clc;
|
||||
|
||||
%% Intialize Laplace variable
|
||||
s = zpk('s');
|
||||
|
||||
%% Path for functions, data and scripts
|
||||
addpath('./mat/'); % Path for data
|
||||
|
||||
%% Simscape Model - Nano Hexapod
|
||||
addpath('./STEPS/')
|
||||
|
||||
%% Colors for the figures
|
||||
colors = colororder;
|
||||
|
||||
%% Frequency Vector
|
||||
freqs = logspace(1, 3, 1000);
|
||||
|
||||
% Bragg Angle
|
||||
|
||||
%% Tested bragg angles
|
||||
bragg = linspace(5, 80, 1000); % Bragg angle [deg]
|
||||
d_off = 10.5e-3; % Wanted offset between x-rays [m]
|
||||
|
||||
%% Vertical Jack motion as a function of Bragg angle
|
||||
dz = d_off./(2*cos(bragg*pi/180));
|
||||
|
||||
%% Jack motion as a function of Bragg angle
|
||||
figure;
|
||||
plot(bragg, 1e3*dz)
|
||||
xlabel('Bragg angle [deg]'); ylabel('Jack Motion [mm]');
|
||||
|
||||
|
||||
|
||||
% #+name: fig:jack_motion_bragg_angle
|
||||
% #+caption: Jack motion as a function of Bragg angle
|
||||
% #+RESULTS:
|
||||
% [[file:figs/jack_motion_bragg_angle.png]]
|
||||
|
||||
|
||||
%% Required Jack stroke
|
||||
ans = 1e3*(dz(end) - dz(1))
|
||||
|
||||
|
||||
|
||||
% #+name: fig:schematic_sensor_jacobian_inverse_kinematics
|
||||
% #+caption: Inverse Kinematics - Interferometers
|
||||
% #+RESULTS:
|
||||
% [[file:figs/schematic_sensor_jacobian_inverse_kinematics.png]]
|
||||
|
||||
|
||||
% From the Figure [[fig:sensor_111_crystal_points]], the inverse kinematics can be solved as follow (for small motion):
|
||||
% \begin{equation}
|
||||
% \bm{J}_{s,111}
|
||||
% =
|
||||
% \begin{bmatrix}
|
||||
% 1 & 0.07 & -0.015 \\
|
||||
% 1 & 0 & 0.015 \\
|
||||
% 1 & -0.07 & -0.015
|
||||
% \end{bmatrix}
|
||||
% \end{equation}
|
||||
|
||||
|
||||
%% Sensor Jacobian matrix for 111 crystal
|
||||
J_s_111 = [1, 0.07, -0.015
|
||||
1, 0, 0.015
|
||||
1, -0.07, -0.015];
|
||||
|
||||
|
||||
|
||||
% #+name: fig:schematic_sensor_jacobian_inverse_kinematics
|
||||
% #+caption: Inverse Kinematics - Actuators
|
||||
% #+RESULTS:
|
||||
% [[file:figs/schematic_actuator_jacobian_inverse_kinematics.png]]
|
||||
|
||||
% Based on the geometry in Figure [[fig:actuator_jacobian_111_points]], we obtain:
|
||||
% \begin{equation}
|
||||
% \bm{J}_{a,111}
|
||||
% =
|
||||
% \begin{bmatrix}
|
||||
% 1 & 0.14 & -0.1525 \\
|
||||
% 1 & 0.14 & 0.0675 \\
|
||||
% 1 & -0.14 & -0.0425
|
||||
% \end{bmatrix}
|
||||
% \end{equation}
|
||||
|
||||
|
||||
%% Actuator Jacobian - 111 crystal
|
||||
J_a_111 = [1, 0.14, -0.1525
|
||||
1, 0.14, 0.0675
|
||||
1, -0.14, -0.0425];
|
||||
|
||||
save('mat/dcm_kinematics.mat', 'J_a_111', 'J_s_111')
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Reference in New Issue
Block a user