add all files

A2-nass-rotating-3dof-model/rotating_2_iff_pure_int.m

@@ -0,0 +1,86 @@
+%% 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
+addpath('./src/'); % Path for Functions
+
+%% Colors for the figures
+colors = colororder;
+
+%% Simscape model name
+mdl = 'rotating_model';
+
+%% Load "Generic" system dynamics
+load('rotating_generic_plants.mat', 'Gs', 'Wzs');
+
+%% Bode plot of the direct and coupling term for Integral Force Feedback - Effect of rotation
+freqs = logspace(-2, 1, 1000);
+
+Wz_i = [1,3,4];
+
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+% Magnitude
+ax1 = nexttile([2, 1]);
+hold on;
+for i = 1:length(Wz_i)
+    plot(freqs, abs(squeeze(freqresp(Gs{Wz_i(i)}('fu', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(i,:), ...
+         'DisplayName', sprintf('$\\Omega = %.1f \\omega_0 $', Wzs(Wz_i(i))),'MarkerSize',8);
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]');
+ylim([1e-3, 1e2]);
+leg = legend('location', 'northwest', 'FontSize', 8);
+
+ax2 = nexttile;
+hold on;
+for i = 1:length(Wz_i)
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{Wz_i(i)}('fu', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(i,:))
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
+yticks(-180:90:180);
+ylim([0 180]);
+xticks([1e-2,1e-1,1,1e1])
+xticklabels({'$0.01 \omega_0$', '$0.1 \omega_0$', '$\omega_0$', '$10 \omega_0$'})
+
+linkaxes([ax1,ax2],'x');
+xlim([freqs(1), freqs(end)]);
+
+%% Root Locus for the Decentralized Integral Force Feedback controller
+Kiff = 1/s*eye(2);
+
+gains = logspace(-2, 4, 300);
+Wz_i = [1,3,4];
+
+figure;
+hold on;
+for i = 1:length(Wz_i)
+    plot(real(pole(Gs{Wz_i(i)}({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)),  imag(pole(Gs{Wz_i(i)}({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), 'x', 'color', colors(i,:), ...
+         'DisplayName', sprintf('$\\Omega = %.1f \\omega_0 $', Wzs(Wz_i(i))),'MarkerSize',8);
+    plot(real(tzero(Gs{Wz_i(i)}({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)),  imag(tzero(Gs{Wz_i(i)}({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), 'o', 'color', colors(i,:), ...
+         'HandleVisibility', 'off','MarkerSize',8);
+    for g = gains
+        cl_poles = pole(feedback(Gs{Wz_i(i)}({'fu', 'fv'}, {'Fu', 'Fv'}), g*Kiff, -1));
+        plot(real(cl_poles), imag(cl_poles), '.', 'color', colors(i,:), ...
+             'HandleVisibility', 'off','MarkerSize',4);
+    end
+end
+hold off;
+axis square;
+xlim([-1.8, 0.2]); ylim([0, 2]);
+xticks([-1, 0])
+xticklabels({'-$\omega_0$', '$0$'})
+yticks([0, 1, 2])
+yticklabels({'$0$', '$\omega_0$', '$2 \omega_0$'})
+
+xlabel('Real Part'); ylabel('Imaginary Part');
+leg = legend('location', 'northwest', 'FontSize', 8);
+leg.ItemTokenSize(1) = 8;