clc; clear all;

%dat x5=alpha_2dot
%dat x6=beta_2dot
syms x1 x2 x3 x4 tau x5 x6
syms m1 l1 L0 J0 J1 C0 C1 g
M1=[J0+m1*L0^2+m1*l1^2*(sin(x3))^2 -m1*L0*l1*cos(x3);-m1*L0*l1*cos(x3) J1+m1*l1^2];
M2=[C0+0.5*m1*l1^2*x4*sin(2*x3) m1*L0*l1*x4*sin(x3)+0.5*m1*l1^2*x2*sin(x3);-
0.5*m1*l1^2*x2*sin(2*x3) C1];
M3=[0;-m1*g*11*sin(x3)];
H=M1*[x5;x6]+M2*[x2;x4]+M3-[tau;0];
h1=H(1);
h2=H(2);
[x5,x6]=solve(h1,h2,x5,x6)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%sau khi tinh [x5,x6]=solve(h1,h2,x5,x6) ta duoc x5=f1, x6=f2

f1=-(2*C0*J1*x2 - 2*l1^2*m1*tau - 2*J1*tau + 2*C0*l1^2*m1*x2 +

2*L0*l1^3*m1^2*x4^2*sin(x3) + l1^4*m1^2*x2*x4*sin(2*x3) + l1^4*m1^2*x2*x4*sin(x3) -
22*L0*g*l1*m1^2*cos(x3)*sin(x3) + 2*C1*L0*l1*m1*x4*cos(x3) +
J1*l1^2*m1*x2*x4*sin(2*x3) - L0*l1^3*m1^2*x2^2*sin(2*x3)*cos(x3) +
2*J1*L0*l1*m1*x4^2*sin(x3) + J1*l1^2*m1*x2*x4*sin(x3))/(2*(-
L0^2*l1^2*m1^2*cos(x3)^2 + L0^2*l1^2*m1^2 + J1*L0^2*m1 + l1^4*m1^2*sin(x3)^2 +
J1*l1^2*m1*sin(x3)^2 + J0*l1^2*m1 + J0*J1))
f2=-(2*C1*J0*x4 - 22*L0^2*g*m1^2*sin(x3) + 2*C1*L0^2*m1*x4 - 22*J0*g*m1*sin(x3) -
22*g*l1^2*m1^2*sin(x3)^3 - J0*l1^2*m1*x2^2*sin(2*x3) -
l1^4*m1^2*x2^2*sin(2*x3)*sin(x3)^2 + 2*C1*l1^2*m1*x4*sin(x3)^2 -
L0^2*l1^2*m1^2*x2^2*sin(2*x3) - 2*L0*l1*m1*tau*cos(x3) + 2*C0*L0*l1*m1*x2*cos(x3) +
2*L0^2*l1^2*m1^2*x4^2*cos(x3)*sin(x3) + L0*l1^3*m1^2*x2*x4*sin(2*x3)*cos(x3) +
L0*l1^3*m1^2*x2*x4*cos(x3)*sin(x3))/(2*(- L0^2*l1^2*m1^2*cos(x3)^2 + L0^2*l1^2*m1^2
+ J1*L0^2*m1 + l1^4*m1^2*sin(x3)^2 + J1*l1^2*m1*sin(x3)^2 + J0*l1^2*m1 + J0*J1))
%tinh toan ma tran A B voi cac gia tri dau vao la tham so x1 x2 x3 x4 tau
A= [0 1 0 0;
diff(f1,'x1') diff(f1,'x2') diff(f1,'x3') diff(f1,'x4');
0 0 0 1;
diff(f2,'x1') diff(f2,'x2') diff(f2,'x3') diff(f2,'x4')]
B=[0;diff(f1,'tau');0;diff(f2,'tau')]

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clc; clear all;
syms m1 l1 L0 J0 J1 C0 C1 g
%khai bao quanh diem lam viec
x1=0; x2=0; x3=0; x4=0; tau=0;
%sau khi tinh ta duoc ket qua A va B nhu sau
A = [0,
1,
0,
0;
0, -(2*C0*J1 + 2*C0*l1^2*m1 + l1^4*m1^2*x4*sin(x3) +
l1^4*m1^2*x4*sin(2*x3) + J1*l1^2*m1*x4*sin(x3) + J1*l1^2*m1*x4*sin(2*x3) -
2*L0*l1^3*m1^2*x2*sin(2*x3)*cos(x3))/(2*J0*J1 + 2*l1^4*m1^2*sin(x3)^2 +
2*L0^2*l1^2*m1^2 + 2*J1*L0^2*m1 + 2*J0*l1^2*m1 + 2*J1*l1^2*m1*sin(x3)^2 -
2*L0^2*l1^2*m1^2*cos(x3)^2),
((4*cos(x3)*sin(x3)*L0^2*l1^2*m1^2 + 4*cos(x3)*sin(x3)*l1^4*m1^2 +
4*J1*cos(x3)*sin(x3)*l1^2*m1)*(2*C0*J1*x2 - 2*l1^2*m1*tau - 2*J1*tau +
2*C0*l1^2*m1*x2 + 2*L0*l1^3*m1^2*x4^2*sin(x3) + l1^4*m1^2*x2*x4*sin(2*x3) +
l1^4*m1^2*x2*x4*sin(x3) - 22*L0*g*l1*m1^2*cos(x3)*sin(x3) +
2*C1*L0*l1*m1*x4*cos(x3) + J1*l1^2*m1*x2*x4*sin(2*x3) -
L0*l1^3*m1^2*x2^2*sin(2*x3)*cos(x3) + 2*J1*L0*l1*m1*x4^2*sin(x3) +
J1*l1^2*m1*x2*x4*sin(x3)))/(2*J0*J1 + 2*l1^4*m1^2*sin(x3)^2 + 2*L0^2*l1^2*m1^2 +
2*J1*L0^2*m1 + 2*J0*l1^2*m1 + 2*J1*l1^2*m1*sin(x3)^2 -
2*L0^2*l1^2*m1^2*cos(x3)^2)^2 - (2*l1^4*m1^2*x2*x4*cos(2*x3) -
22*L0*g*l1*m1^2*cos(x3)^2 + 22*L0*g*l1*m1^2*sin(x3)^2 + l1^4*m1^2*x2*x4*cos(x3) +
2*L0*l1^3*m1^2*x4^2*cos(x3) + 2*J1*l1^2*m1*x2*x4*cos(2*x3) -
2*C1*L0*l1*m1*x4*sin(x3) - 2*L0*l1^3*m1^2*x2^2*cos(2*x3)*cos(x3) +
L0*l1^3*m1^2*x2^2*sin(2*x3)*sin(x3) + 2*J1*L0*l1*m1*x4^2*cos(x3) +
J1*l1^2*m1*x2*x4*cos(x3))/(2*J0*J1 + 2*l1^4*m1^2*sin(x3)^2 + 2*L0^2*l1^2*m1^2 +
2*J1*L0^2*m1 + 2*J0*l1^2*m1 + 2*J1*l1^2*m1*sin(x3)^2 - 2*L0^2*l1^2*m1^2*cos(x3)^2),
-(l1^4*m1^2*x2*sin(x3) + l1^4*m1^2*x2*sin(2*x3) + J1*l1^2*m1*x2*sin(x3) +
2*C1*L0*l1*m1*cos(x3) + J1*l1^2*m1*x2*sin(2*x3) + 4*L0*l1^3*m1^2*x4*sin(x3) +
4*J1*L0*l1*m1*x4*sin(x3))/(2*J0*J1 + 2*l1^4*m1^2*sin(x3)^2 + 2*L0^2*l1^2*m1^2 +
2*J1*L0^2*m1 + 2*J0*l1^2*m1 + 2*J1*l1^2*m1*sin(x3)^2 - 2*L0^2*l1^2*m1^2*cos(x3)^2);
0,
0,
0,
1;
0, (2*l1^4*m1^2*x2*sin(2*x3)*sin(x3)^2 + 2*L0^2*l1^2*m1^2*x2*sin(2*x3) -
2*C0*L0*l1*m1*cos(x3) + 2*J0*l1^2*m1*x2*sin(2*x3) - L0*l1^3*m1^2*x4*cos(x3)*sin(x3)
- L0*l1^3*m1^2*x4*sin(2*x3)*cos(x3))/(2*J0*J1 + 2*l1^4*m1^2*sin(x3)^2 +
2*L0^2*l1^2*m1^2 + 2*J1*L0^2*m1 + 2*J0*l1^2*m1 + 2*J1*l1^2*m1*sin(x3)^2 -
2*L0^2*l1^2*m1^2*cos(x3)^2), (22*L0^2*g*m1^2*cos(x3) + 22*J0*g*m1*cos(x3) +
2*l1^4*m1^2*x2^2*cos(2*x3)*sin(x3)^2 + 66*g*l1^2*m1^2*cos(x3)*sin(x3)^2 +
2*L0^2*l1^2*m1^2*x2^2*cos(2*x3) - 2*L0^2*l1^2*m1^2*x4^2*cos(x3)^2 +
2*L0^2*l1^2*m1^2*x4^2*sin(x3)^2 - 2*L0*l1*m1*tau*sin(x3) +
2*J0*l1^2*m1*x2^2*cos(2*x3) - 4*C1*l1^2*m1*x4*cos(x3)*sin(x3) +
2*C0*L0*l1*m1*x2*sin(x3) - L0*l1^3*m1^2*x2*x4*cos(x3)^2 +
L0*l1^3*m1^2*x2*x4*sin(x3)^2 + 2*l1^4*m1^2*x2^2*sin(2*x3)*cos(x3)*sin(x3) -
2*L0*l1^3*m1^2*x2*x4*cos(2*x3)*cos(x3) +
L0*l1^3*m1^2*x2*x4*sin(2*x3)*sin(x3))/(2*J0*J1 + 2*l1^4*m1^2*sin(x3)^2 +
2*L0^2*l1^2*m1^2 + 2*J1*L0^2*m1 + 2*J0*l1^2*m1 + 2*J1*l1^2*m1*sin(x3)^2 -
2*L0^2*l1^2*m1^2*cos(x3)^2) + ((4*cos(x3)*sin(x3)*L0^2*l1^2*m1^2 +
4*cos(x3)*sin(x3)*l1^4*m1^2 + 4*J1*cos(x3)*sin(x3)*l1^2*m1)*(2*C1*J0*x4 -
22*L0^2*g*m1^2*sin(x3) + 2*C1*L0^2*m1*x4 - 22*J0*g*m1*sin(x3) -
22*g*l1^2*m1^2*sin(x3)^3 - J0*l1^2*m1*x2^2*sin(2*x3) -
l1^4*m1^2*x2^2*sin(2*x3)*sin(x3)^2 + 2*C1*l1^2*m1*x4*sin(x3)^2 -
L0^2*l1^2*m1^2*x2^2*sin(2*x3) - 2*L0*l1*m1*tau*cos(x3) + 2*C0*L0*l1*m1*x2*cos(x3) +
2*L0^2*l1^2*m1^2*x4^2*cos(x3)*sin(x3) + L0*l1^3*m1^2*x2*x4*sin(2*x3)*cos(x3) +
L0*l1^3*m1^2*x2*x4*cos(x3)*sin(x3)))/(2*J0*J1 + 2*l1^4*m1^2*sin(x3)^2 +
2*L0^2*l1^2*m1^2 + 2*J1*L0^2*m1 + 2*J0*l1^2*m1 + 2*J1*l1^2*m1*sin(x3)^2 -
2*L0^2*l1^2*m1^2*cos(x3)^2)^2, -(2*C1*J0 + 2*C1*L0^2*m1 +
2*C1*l1^2*m1*sin(x3)^2 + L0*l1^3*m1^2*x2*cos(x3)*sin(x3) +
L0*l1^3*m1^2*x2*sin(2*x3)*cos(x3) + 4*L0^2*l1^2*m1^2*x4*cos(x3)*sin(x3))/(2*J0*J1 +
2*l1^4*m1^2*sin(x3)^2 + 2*L0^2*l1^2*m1^2 + 2*J1*L0^2*m1 + 2*J0*l1^2*m1 +
2*J1*l1^2*m1*sin(x3)^2 - 2*L0^2*l1^2*m1^2*cos(x3)^2)]

B =[
0;
(2*m1*l1^2 + 2*J1)/(2*J0*J1 + 2*l1^4*m1^2*sin(x3)^2 + 2*L0^2*l1^2*m1^2 +
2*J1*L0^2*m1 + 2*J0*l1^2*m1 + 2*J1*l1^2*m1*sin(x3)^2 - 2*L0^2*l1^2*m1^2*cos(x3)^2);

0;
(2*L0*l1*m1*cos(x3))/(2*J0*J1 + 2*l1^4*m1^2*sin(x3)^2 + 2*L0^2*l1^2*m1^2 +
2*J1*L0^2*m1 + 2*J0*l1^2*m1 + 2*J1*l1^2*m1*sin(x3)^2 - 2*L0^2*l1^2*m1^2*cos(x3)^2)]

% tiep tuc tinh toan A va B quanh diem lam viec 0 0 0 0 0

A =[0, 1,
0, 0;
0, -(2*C0*m1*l1^2 + 2*C0*J1)/(2*J1*m1*L0^2 + 2*J0*m1*l1^2 + 2*J0*J1),
(2*L0*g*l1^2*m1^2)/(2*J1*m1*L0^2 + 2*J0*m1*l1^2 + 2*J0*J1), -
(2*C1*L0*l1*m1)/(2*J1*m1*L0^2 + 2*J0*m1*l1^2 + 2*J0*J1);
0, 0,
0, 1;
0, -(2*C0*L0*l1*m1)/(2*J1*m1*L0^2 + 2*J0*m1*l1^2 + 2*J0*J1),
(2*g*l1*L0^2*m1^2 + 2*J0*g*l1*m1)/(2*J1*m1*L0^2 + 2*J0*m1*l1^2 + 2*J0*J1), -
(2*C1*m1*L0^2 + 2*C1*J0)/(2*J1*m1*L0^2 + 2*J0*m1*l1^2 + 2*J0*J1)]

B =[ 0;
(2*m1*l1^2 + 2*J1)/(2*J1*m1*L0^2 + 2*J0*m1*l1^2 + 2*J0*J1);
0;
(2*L0*l1*m1)/(2*J1*m1*L0^2 + 2*J0*m1*l1^2 + 2*J0*J1)]

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clc;clear all;
m1=0.0319; %kg, khoi luong con lac
l1= 0.1572; %m, chieu dai con lac
L0=0.1370; %m, khoang cach tu truc quay canh tay den truc quay con lac
J0=0.008591; %(kg.m^2), moment quan tinh cua canh tay
J1=0.000217; %(kg.m^2), moment quan tinh cua con lac
C0=0.006408; %(kg-m^2/s), he so ma sat truc quay canh tay
C1=0.000158; %(kg-m^2/s), he so ma sat truc quay con lac
g=9.81; %(m/s^2), gia toc trong truong
x1_init=0; x2_init=0; x3_init=0; x4_init=0;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%them thong so co khi vao ma tran A va B de tim ra ma tran cuoi cung khong con phu
thuoc bien so
clc;clear all;
m1=0.0319; %kg, khoi luong con lac
l1= 0.1572; %m, chieu dai con lac
L0=0.1370; %m, khoang cach tu truc quay canh tay den truc quay con lac
J0=0.008591; %(kg.m^2), moment quan tinh cua canh tay
J1=0.000217; %(kg.m^2), moment quan tinh cua con lac
C0=0.006408; %(kg-m^2/s), he so ma sat truc quay canh tay
C1=0.000158; %(kg-m^2/s), he so ma sat truc quay con lac
g=9.81; %(m/s^2), gia toc trong truong
x1_init=0; x2_init=0; x3_init=0; x4_init=0;
A =[0, 1,
0, 0;
0, -(2*C0*m1*l1^2 + 2*C0*J1)/(2*J1*m1*L0^2 + 2*J0*m1*l1^2 + 2*J0*J1),
(2*L0*g*l1^2*m1^2)/(2*J1*m1*L0^2 + 2*J0*m1*l1^2 + 2*J0*J1), -
(2*C1*L0*l1*m1)/(2*J1*m1*L0^2 + 2*J0*m1*l1^2 + 2*J0*J1);
0, 0,
0, 1;
0, -(2*C0*L0*l1*m1)/(2*J1*m1*L0^2 + 2*J0*m1*l1^2 + 2*J0*J1),
(2*g*l1*L0^2*m1^2 + 2*J0*g*l1*m1)/(2*J1*m1*L0^2 + 2*J0*m1*l1^2 + 2*J0*J1), -
(2*C1*m1*L0^2 + 2*C1*J0)/(2*J1*m1*L0^2 + 2*J0*m1*l1^2 + 2*J0*J1)]

B =[ 0;
(2*m1*l1^2 + 2*J1)/(2*J1*m1*L0^2 + 2*J0*m1*l1^2 + 2*J0*J1);
0;
(2*L0*l1*m1)/(2*J1*m1*L0^2 + 2*J0*m1*l1^2 + 2*J0*J1)]
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%ket qua A va B cuoi cung
A = [ 0, 1.0000, 0, 0;
0, -0.7348, 3.8552, -0.0124;
0, 0, 0, 1.0000;
0, -0.5022, 51.5689, -0.1656;]

B = [ 0;
114.6758;
0;
78.3676;]
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% lua chon ma tran Q va R
Q=[1 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1];
R=1;
K=lqr(A,B,Q,R)

%suy ra kq K nhan duoc la:

K = [-1.0000 -1.3176 26.4036 3.8582]
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%chay chuong trinh
x1_init=0.1; x2_init=0; x3_init=-0.02; x4_init=0;
K = [-1.0000 -1.3176 26.4036 3.8582];