102 lines
2.6 KiB
Matlab
102 lines
2.6 KiB
Matlab
% 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)]);
|