%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Calculate A,B,C,D Matrices of DIP System
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Initial commands to clear memory
% and specify formatting
clc;clear all;format compact;
% Give values for parameters
m0=1;m1=0.3;m2=0.6;L1=0.2;L2=0.4;g=9.81;
% Linearize system
theta1=0;theta2=0;
% Calculate elements of matrices
d1=m0+m1+m2;
d2=(1/2*m1+m2)*L1;
d3=1/2*m2*L2;
d4=(1/3*m1+m2)*L1^2;
d5=1/2*m2*L1*L2;
d6=1/3*m2*L2^2;
f1=(1/2*m1+m2)*L1*g;
f2=1/2*m2*L2*g;
% Calculate initial matrices
F=[d1 d2*cos(theta1) d3*cos(theta2);
    d2*cos(theta1) d4 d5*cos(theta1-theta2);
    d3*cos(theta2) d5*cos(theta1-theta2) d6];
G=[0;-f1*sin(theta1);-f2*sin(theta2)];
dG=[0 0 0;0 -f1*cos(theta1) 0;0 0 -f2*cos(theta2)];
H=transpose([1 0 0]);
% Calculate A,B,C,D
A=[zeros(3) eye(3);
    -inv(F)*dG zeros(3)]
B=[zeros(3,1);
    inv(F)*H]
C=[eye(6)];
D=[0];
% Create State Space Model of DIP
sys=ss(A,B,C,D);
% tf(sys);
% co=ctrb(sys);
% rank(co);
% Co=[B A*B A^2*B A^3*B A^4*B A^5*B]
% rank(Co);
% rank(A)
% Q=C'*C
Q=diag([100, 100, 200, 1, 1, 1])
% Q1=diag([100, 100, 200, 0, 0, 0])
% Q2=diag([100, 100, 200, 1, 1, 1])
% Q3=diag([100, 600, 800, 1, 1, 1])
R=1;
% R2=100;
K=lqr(A,B,Q,R)
% K1=lqr(A,B,Q1,R)
% K2=lqr(A,B,Q2,R)
% K3=lqr(A,B,Q3,R)
KK=sqrt(sumsqr(K))
% F=K/KK
Ac = [(A-B*K)];
% Ac1= [(A-B*K1)];
% Ac2= [(A-B*K2)];
% Ac3= [(A-B*K3)];
Bc = [B];
Cc = [C];
Dc = [D];
sys_cl=ss(Ac,Bc,Cc,Dc);
% t = 0:0.01:10;
% r =0.2*ones(size(t));
% [y,t,x]=initial(sys_cl,[0;deg2rad(10);deg2rad(-10);0;0;0],7);
% [y,t,x]=lsim(sys_cl,r,t)
% [AX,H1,H2] = plotyy(t,y(:,1),t,y(:,2),'plot');
% hold on
% plot(t,y(:,1))
% hold on
% plot(t,y(:,2))
% hold on
% plot(t,y(:,6))
% y(:,2:3)=rad2deg(y(:,2:3));
% y(:,5:6)=rad2deg(y(:,5:6));
% Plot the data
% close all
% for i=1:1:6
% subplot(2,3,i)
% plot(t,y(:,i))
% grid
% end