[REFACTORING] Huge changes, WIP.

Control/cl_tf.m

@@ -0,0 +1,23 @@
+%%
+clear; close all; clc;
+
+%% Load Plant
+load('./mat/G_xg_to_d.mat', 'G_xg_to_d');
+load('./mat/G_f_to_d.mat', 'G_f_to_d');
+load('./mat/controller.mat', 'K');
+
+%% 
+S = minreal(inv(tf(eye(6))+G_f_to_d*K));
+T = minreal((tf(eye(6))+G_f_to_d*K)\G_f_to_d*K);
+
+bodeFig({S(1,1), T(1,1)})
+legend({'$S_x$', '$T_x$'})
+
+bodeFig({S(2,2), T(2,2)})
+legend({'$S_y$', '$T_y$'})
+
+bodeFig({S(3,3), T(3,3)})
+legend({'$S_z$', '$T_z$'})
+
+%%
+save('./mat/T_S.mat', 'S', 'T');

Control/control_y.m

@@ -1,34 +0,0 @@
-
-%%
-load('./mat/G_f_to_d.mat', 'G_f_to_d');
-
-%%
-G = G_f_to_d(2, 2);
-
-%% Try sisotool
-sisotool(G)
-
-%%
-gain = 1e9;
-
-%%
-bodeFig({gain*G}, struct('phase', true))
-
-%%
-[~,~,~,Wpm] = margin(gain*G);
-
-Wpm = 200*2*pi;
-
-%%
-s = tf('s');
-C = gain*(s/(0.2*Wpm)+1)/(s/(10*Wpm)+1)/(1+s/(2*pi*100));%*(s+2*pi*10)/(s+2*pi*0.0001);
-
-%% Compute Closed loop transfer function
-bodeFig({C*G}, struct('phase', true))
-
-%%
-K = tf(zeros(6));
-K(2,2) = C;
-
-%%
-save('./mat/controller.mat', 'K')

Control/generate_control.m

@@ -0,0 +1,34 @@
+%%
+clear; close all; clc;
+
+%% Load Plant
+load('./mat/G_f_to_d.mat', 'G_20');
+
+%% Load previously generated controllers
+load('./mat/control_K_tx.mat', 'K_tx');
+load('./mat/control_K_ty.mat', 'K_ty');
+load('./mat/control_K_tz.mat', 'K_tz');
+
+%%
+sisotool('bode', G_20(1, 1), K_tx);
+K_tx = C;
+save('./mat/control_K_tx.mat', 'K_tx');
+
+%%
+sisotool('bode', G_20(2, 2), K_ty);
+K_ty = C;
+save('./mat/control_K_ty.mat', 'K_ty');
+
+%%
+sisotool('bode', G_20(3, 3), K_tz);
+K_tz = C;
+save('./mat/control_K_tz.mat', 'K_tz');
+
+%%
+K = tf(zeros(6));
+K(1,1) = K_tx;
+K(2,2) = K_ty;
+K(3,3) = K_tz;
+
+%% Save the MIMO control
+save('./mat/controller.mat', 'K');