%%
clear; close all; clc;

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

%%
fs = 10;

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');