clear
format compact

% ---------------------------------
% パラメータを実数のシンボリックな変数として定義
syms a1 b1 alp5 alp2 alp3 mu2 theta1_e real

% ---------------------------------
% 行列A, Bの計算
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         ];

% ---------------------------------
% 可制御行列 MR の計算（文字を含む計算には関数 ctrb は使用できない）
MR = [ B  A*B  A^2*B  A^3*B ];

% ---------------------------------
disp('===========================================')
disp(' ')

% 可制御行列 MR の行列式 det(MR) の計算
disp('***** det(MR) *****')
det_MR = det(MR);
det_MR = simplify(det_MR)
disp(' ')

% パラメータの数値の定義
pendubot_para  %% M ファイルの読み込み

% det(MR) にパラメータの数値を代入
det_MR_num = subs(det_MR, {'mu2', 'alp2', 'alp3', 'alp4', 'alp5'},{mu2, alp2, alp3, alp4, alp5});

% det(MR) = 0 の解 theta1_e を計算し，倍精度で表示
disp('***** det(MR) = 0 の解 theta1_e [deg] *****')
double(solve(det_MR_num, theta1_e))*(180/pi)
disp(' ')


% ---------------------------------
disp('===========================================')
disp('mu2 = 0 のとき ...')
disp(' ')

% 振子の粘性摩擦が 0 (mu2 = 0) のときの det(MR) の計算
disp('***** det(MR) (mu2 = 0) *****')
det_MR_0 = subs(det_MR,'mu2',0)
disp(' ')

% det(MR) = 0 の解 theta1_e を計算し，倍精度で表示
disp('***** det(MR) = 0 (mu2 = 0) の解 theta1_e [deg] *****')
double(solve(det_MR_0, theta1_e))*(180/pi)
disp(' ')
