% 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, '/control_system'], 1, 'openinput'); io_i = io_i + 1; %% Outputs % Strain Gauges {3x1} [m] io(io_i) = linio([mdl, '/DCM'], 2, '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: % | 4.4443e-09 | 1.0339e-13 | 3.774e-14 | % | 1.0339e-13 | 4.4443e-09 | 3.774e-14 | % | 3.7792e-14 | 3.7792e-14 | 4.4444e-09 | %% Bode plot for the plant (strain gauge output) 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', 'southeast', 'FontSize', 8, 'NumColumns', 2); ylim([1e-14, 1e-7]); 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, 0]); linkaxes([ax1,ax2],'x'); xlim([freqs(1), freqs(end)]); % Relative Active Damping Krad_g1 = eye(3)*s/(s^2/(2*pi*500)^2 + 2*s/(2*pi*500) + 1); % As can be seen in Figure [[fig:relative_damping_root_locus]], very little damping can be added using relative damping strategy using strain gauges. %% Root Locus for IFF gains = logspace(3, 8, 200); figure; hold on; plot(real(pole(G_sg)), imag(pole(G_sg)), 'x', 'color', colors(1,:), ... 'DisplayName', '$g = 0$'); plot(real(tzero(G_sg)), imag(tzero(G_sg)), 'o', 'color', colors(1,:), ... 'HandleVisibility', 'off'); for g = gains clpoles = pole(feedback(G_sg, g*Krad_g1, -1)); plot(real(clpoles), imag(clpoles), '.', 'color', colors(1,:), ... 'HandleVisibility', 'off'); end % Optimal gain g = 2e5; clpoles = pole(feedback(G_sg, g*Krad_g1, -1)); plot(real(clpoles), imag(clpoles), 'x', 'color', colors(2,:), ... 'DisplayName', sprintf('$g=%.0e$', g)); hold off; xlim([-6, 0]); ylim([0, 2700]); xlabel('Real Part'); ylabel('Imaginary Part'); legend('location', 'northwest'); % #+name: fig:relative_damping_root_locus % #+caption: Root Locus for the relative damping control % #+RESULTS: % [[file:figs/relative_damping_root_locus.png]] Krad = -g*Krad_g1; % Damped Plant % The controller is implemented on Simscape, and the damped plant is identified. %% 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('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 Relative Active Damping controller.type = 2; % RAD 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)]);