nass-simscape/control/matlab-old/control_generate.m

%%
clear; close all; clc;

%% Load Plant
load('./mat/G.mat', 'G_light_vc', 'G_light_pz', 'G_heavy_vc', 'G_heavy_pz');

%%
s = tf('s');

%%
% bodeFig({minreal(G_light_vc.G_cart(1, 1))}, struct('phase', true));

%%
% sisotool(minreal(G_light_vc.G_cart(6, 6)))

K_light_vc = tf(zeros(6));
K_light_vc(1, 1) = 3.4802e06*(s+0.6983)*(s+131.6)/((s+5.306e-06)*(s+577.4));
K_light_vc(2, 2) = 5.3292e06*(s+0.6722)*(s+58.23)/((s+3.628e-06)*(s+734.6));
K_light_vc(3, 3) = 2.1727e06*(s+66.94)*(s+3.522)/((s+530.6)*(s+0.0006859));
K_light_vc(4, 4) = 6.4807e06*(s+2.095)*(s+111.1)/((s+0.0001246)*(s+577.4));
K_light_vc(5, 5) = 2.5184e06*(s+5.582)*(s+48.76)/((s+0.01558)*(s+453.8));
K_light_vc(6, 6) = 1.022e06*(s+6.152)*(s+40.15)/((s+0.001945)*(s+589.7));

%%
i = 6;
bodeFig({-G_light_vc.G_plant(i, i)*K_light_vc(i, i)}, struct('phase', true))

%%
% sisotool(minreal(G_light_pz.G_plant(1, 1)))

K_light_pz = tf(zeros(6));
% K_light_pz(1, 1) = 3.4802e06*(s+0.6983)*(s+131.6)/((s+5.306e-06)*(s+577.4));
% K_light_pz(2, 2) = 5.3292e06*(s+0.6722)*(s+58.23)/((s+3.628e-06)*(s+734.6));
% K_light_pz(3, 3) = 2.1727e06*(s+66.94)*(s+3.522)/((s+530.6)*(s+0.0006859));
% K_light_pz(4, 4) = 6.4807e06*(s+2.095)*(s+111.1)/((s+0.0001246)*(s+577.4));
% K_light_pz(5, 5) = 2.5184e06*(s+5.582)*(s+48.76)/((s+0.01558)*(s+453.8));
% K_light_pz(6, 6) = 1.022e06*(s+6.152)*(s+40.15)/((s+0.001945)*(s+589.7));

%%
K_heavy_vc = tf(zeros(6));
K_heavy_pz = tf(zeros(6));


%%
save('./mat/K_fb.mat', 'K_light_vc', 'K_light_pz', 'K_heavy_vc', 'K_heavy_pz');

%% Automatic generation of controllers
% fs = 100;
%
% K_light_vc = generateDiagPidControl(G_light_vc.G_cart, fs);
% K_light_pz = generateDiagPidControl(G_light_pz.G_cart, fs);
%
% K_heavy_vc = generateDiagPidControl(G_heavy_vc.G_cart, fs);
% K_heavy_pz = generateDiagPidControl(G_heavy_pz.G_cart, fs);

%% Save the MIMO control
% save('./mat/K_fb.mat', 'K_light_vc', 'K_light_pz', 'K_heavy_vc', 'K_heavy_pz');