dcm-simscape-model/matlab/dcm_active_damping_strain_gauges.m

% 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)]);