%%
clear; close all; clc;

%% Load Plant
load('./mat/G_iff.mat', 'G_iff_light_vc', 'G_iff_light_pz', 'G_iff_heavy_vc', 'G_iff_heavy_pz');

%%
fs = 10;

K_light_vc_iff = generateDiagPidControl(G_iff_light_vc.G_cart, fs);
K_light_pz_iff = generateDiagPidControl(G_iff_light_pz.G_cart, fs);

K_heavy_vc_iff = generateDiagPidControl(G_iff_heavy_vc.G_cart, fs);
K_heavy_pz_iff = generateDiagPidControl(G_iff_heavy_pz.G_cart, fs);

%% Save the MIMO control
save('./mat/K_fb_iff.mat', 'K_light_vc_iff', 'K_light_pz_iff', 'K_heavy_vc_iff', 'K_heavy_pz_iff');