% Pro ukázku zvýšení robustnosti nelinearitou: Q=diag([200 110 110 0 0 0]);
% th1i  = deg2rad(0); th2i  = deg2rad(13); k1=1.2;

% Nejrobustnější kontroler bez nelinearity:  %Q =diag([5 50 50 700 700 700]);

%clc;
run ../../parametry.m
dt=0.001;
tf= 3.5;
maxpow=20;

%% pocatecni podminky
th1i  = deg2rad(0); 
th2i  = deg2rad(5);
th0i  = 0;

%% definice statespace
%systemove matice kolem [0 0 0 0 0 0]

d0=[d1 d2 d3; d2 d4 d5; d3 d5 d6];
dg=[0 0 0;0 -f1 0; 0 0 -f2];

a=[zeros(3),eye(3);-inv(d0)*dg,zeros(3)];
b=[zeros(3,1);inv(d0)*[1;0;0]];
c=eye(3,6);
d=[0;0;0];
sys=ss(a,b,c,d);

sysd=c2d(sys,dt);

%% návrh LQR
% Q =diag([5 50 50 700 700 700]); %velmi pomalý, jezdí velmi daleko, nejrobustnější
% Q =diag([100 50 50 10 10 10]); %pomalý, jezdí daleko,
% Q =diag([700, 0, 0, 0, 0, 0]); %rychly, jezdí blízko,
% Q=diag([200 110 110 0 0 0]);
% Q=diag([500 50 100 100 100 100]);
 Q=diag([1000 0 0 0 1000 0]);
R =1;
k=dlqr(sysd.a,sysd.b,Q,R);
k1=1.2;
%% simulace

run('doublePendulumSim_model_LQR.slx');
sim('doublePendulumSim_model_LQR.slx');