clc
warning off;

Rs = 6.39; % ohm
ke = 0.1155; %V.s/rad
pp = 3;
Ld = 11.51e-3; % H
Lq = 12.36e-3; % H
load data_for_PS.mat;     % Load data for Custom Power Stage
a0 = [6e-4 0.0054] % [J F]

vysl = fminsearch(@fkrit,a0,optimset('Display','iter','MaxFunEvals',10000,'MaxIter',10000,'TolFun',1e-3,'TolX',1e-3),y_orig);

%% Vykresleni prubehu
J = vysl(1);
F = vysl(2);

set_param('model/PMSM1','J',mat2str(J));
set_param('model/PMSM1','F',mat2str(F));

sim ('model');

figure;
subplot(3,1,1)
plot(t2,Id,'r');
hold on;
plot(t2,Id_model,'b');
title('Id');
xlabel('t [s]');
ylabel('Id [A]');
legend('motor','model');
grid on;

subplot(3,1,2)
plot(t2,Iq,'r');
hold on;
plot(t2,Iq_model,'b');
title('Iq');
xlabel('t [s]');
ylabel('Iq [A]');
legend('motor','model');
grid on;

subplot(3,1,3)
plot(t2,w_r,'r');
hold on;
plot(t2,w_r_model,'b');
title('w_r');
xlabel('t [s]');
ylabel('w_r [rad/s]');
legend('motor','model');
grid on;