% 『初学者のための図解でわかる制御工学 I』
%  基礎編　「可制御性と状態フィードバック」

clear; 
global a1 b1 alp3 alp2 alp5 mu2 theta1_e F

% ---------------------------
% 物理パラメータの定義
m1  = 3.06e-001;
l1  = 1.20e-001;
L1  = 2.27e-001;
J2  = 1.38e-003;
m2  = 1.04e-001;
l2  = 1.75e-001;
mu2 = 1.86e-004;
g   = 9.81e+000;
a1  = 6.25e+000;
b1  = 1.56e+001;
% ----------------------------
% 平衡点(目標点)の定義
theta1_e = 45*pi/180;
theta2_e = 0*pi/180;
% ---------------------------
% 行列A, Bの計算
alp2  = J2 + m2*l2^2;
alp3  = m2*L1*l2;
alp4  = (m1*l1 + m2*L1)*g;
alp5  = m2*l2*g;
A21 = [ 0  0
        0  alp5/alp2 ];
A22 = [ -a1                  0
        (a1*alp3*cos(theta1_e)+mu2)/alp2  -mu2/alp2 ];
B2  = [  b1
        -b1*alp3*cos(theta1_e)/alp2 ];
k = 2; p = 1;
A = [ zeros(k,k)  eye(k)
      A21         A22    ];
B = [ zeros(k,p)
      B2         ];
% ----------------------------
% 最適レギュレータの設計
Q = diag([10 10 1 1]);
R = 1;
F = lqr(A,B,Q,R);    % ゲインの計算
% ----------------------------
% 初期値の設定と時間積分の実行
theta1_0 = 0*pi/180;
theta2_0 = 0*pi/180;
x0 = [theta1_0; theta2_0; 0; 0]; %(theta1_0, theta2_0, dtheta1_0/dt, dtheta2_0/dt)
[T,Y] = ode45(@pendubot_eq,[0 5],x0);
% ----------------------------
% 角度のグラフの図示
plot(T,Y(:,1),'-',T,Y(:,2),'-.')
ylim([-0.1 pi/4+0.1])
xlim([0 3])
set(gca,'fontname','symbol','fontsize',16)
set(gca,'ytick',0:pi/8:pi/4)
set(gca,'yticklabel',{'0','p/8','p/4'})
xlabel('{\it t} [s]','fontname','times','fontsize',18)
ylabel('\theta_1, \theta_2 [rad]','fontname','times','fontsize',18)
hleg1 = legend('\theta_1','\theta_2',2);
set(hleg1,'fontname','times','fontsize',16) 

