%著作権，名誉毀損などに関する法規その他法令上の義務に従うことなど，
%ユーザーご自身の責任において行っていただきますようお願いいたします．
%御利用につき，何らかのトラブルや損失・損害等につきましては一切責任を問わないものとします．
%間違え・不具合等がありましたらご連絡お願い致します．
%
% 作成者：奥村(熊本大学大学院 自然科学研究科 情報電気電子工学専攻 H28年度卒)
%
%
clear
clc
close all

Ti = 0;
Ts = 0.011;
Tf = 20.0;

t = [Ti:Ts:Tf]';

L = size(t,1);

for ein = 0.005:0.001:0.005
%% パラメータ設定
%プラント
m_b_P = 0.280;          %車体の質量[kg]  
m_w_P = 0.043;          %車輪の質量[kg]
R_P = 0.0275;           %車輪の半径[m]
z_P = 0.0380 + ein;     %重心位置[m]
b_P = 0.05;             %トレッド
g = 9.8;                %重力加速度[m/s^2]
I_wa_P = 2.90*10^(-11);
I_wd_P = 1.07*10^(-4);
I_xx_P = 4.15*10^(-4);
I_yy_P = 4.15*10^(-4);
I_zz_P = 1.07*10^(-4);
I_xz_P = I_xx_P - I_zz_P;

%モデル
m_b_Pm = 0.250;         %車体の質量[kg]  
m_w_Pm = 0.043;         %車輪の質量[kg]
R_Pm = 0.0275;          %車輪の半径[m]
z_Pm = 0.0380;          %重心位置[m]
b_Pm = 0.05;            %トレッド
g = 9.8;                %重力加速度[m/s^2]
I_wa_Pm = 2.90*10^(-11);
I_wd_Pm = 1.07*10^(-4);
I_xx_Pm = 4.15*10^(-4);
I_yy_Pm = 4.15*10^(-4);
I_zz_Pm = 1.07*10^(-4);
I_xz_Pm = I_xx_Pm - I_zz_Pm;

L_body = 0.06;

%% 状態空間表現の準備
%プラント
J_a_P = m_b_P*R_P^2 + 2*I_wa_P + 2*m_w_P*R_P^2;
J_b_P = I_yy_P + m_b_P*z_P^2;
J_c_P = m_b_P*z_P^2 + I_xx_P;
J_d_P = I_wa_P + m_w_P*R_P^2;
D_1_P = @(theta_b) (m_b_P^2)*(z_P^2)*(cos(theta_b)^2)*(R_P^2) - (I_yy_P + m_b_P*z_P^2)*J_a_P;
D_2_P = @(theta_b) J_c_P*(R_P^2)*(sin(theta_b)^2) + I_zz_P*(R_P^2)*(cos(theta_b)^2) + 2*I_wd_P*(R_P^2) + 2*(b_P^2)*I_wa_P + 2*(b_P^2)*m_w_P*(R_P^2);
f_21_P = @(theta_b,d_theta_b,d_phi_v)  -(sin(theta_b)*(cos(theta_b)*(d_theta_b^2)*(J_a_P*I_xz_P-(m_b_P^2)*(z_P^2)*(R_P^2))+2*J_d_P*m_b_P*(z_P^2)*cos(theta_b)*(d_phi_v)^2+m_b_P*g*z_P*J_a_P))/D_1_P(theta_b);
f_22_P = @(theta_b,d_theta_b,d_phi_v) z_P*m_b_P*(R_P^2)*sin(theta_b)*(((cos(theta_b)^2)*I_xz_P-J_b_P)*(d_theta_b^2) + (-J_b_P+m_b_P*(z_P^2)*(cos(theta_b)^2))*(d_phi_v)^2 + cos(theta_b)*m_b_P*g*z_P)/D_1_P(theta_b);
f_23_P = @(theta_b,d_theta_b,d_phi_v,v) -(R_P^2)*sin(theta_b)*(d_phi_v)*(m_b_P*z_P*v + 2*cos(theta_b)*d_theta_b*(I_xz_P+m_b_P*(z_P^2)))/D_2_P(theta_b);
g_15_P = @(theta_b) J_a_P + R_P*cos(theta_b)*z_P*m_b_P;
g_16_P = @(theta_b) -R_P*(R_P*cos(theta_b)*z_P*m_b_P+J_b_P);

%モデル
J_a_Pm = m_b_Pm*R_Pm^2 + 2*I_wa_Pm + 2*m_w_Pm*R_Pm^2;
J_b_Pm = I_yy_Pm + m_b_Pm*z_Pm^2;
J_c_Pm = m_b_Pm*z_Pm^2 + I_xx_Pm;
J_d_Pm = I_wa_Pm + m_w_Pm*R_Pm^2;
D_1_Pm = @(theta_b) (m_b_Pm^2)*(z_Pm^2)*(cos(theta_b)^2)*(R_Pm^2) - (I_yy_Pm + m_b_Pm*z_Pm^2)*J_a_Pm;
D_2_Pm = @(theta_b) J_c_Pm*(R_Pm^2)*((sin(theta_b))^2) + I_zz_Pm*(R_Pm^2)*((cos(theta_b))^2) + 2*I_wd_Pm*(R_Pm^2) + 2*(b_Pm^2)*I_wa_Pm + 2*(b_Pm^2)*m_w_Pm*(R_Pm^2);
f_21_Pm = @(theta_b,d_theta_b,d_phi_v)  -(sin(theta_b)*(cos(theta_b)*(d_theta_b^2)*(J_a_Pm*I_xz_Pm-(m_b_Pm^2)*(z_Pm^2)*(R_Pm^2))+2*J_d_Pm*m_b_Pm*(z_Pm^2)*cos(theta_b)*(d_phi_v)^2+m_b_Pm*g*z_Pm*J_a_Pm))/D_1_Pm(theta_b);
f_22_Pm = @(theta_b,d_theta_b,d_phi_v) z_Pm*m_b_Pm*(R_Pm^2)*sin(theta_b)*(((cos(theta_b)^2)*I_xz_Pm-J_b_Pm)*(d_theta_b^2) + (-J_b_Pm+m_b_Pm*(z_Pm^2)*(cos(theta_b)^2))*(d_phi_v)^2 + cos(theta_b)*m_b_Pm*g*z_Pm)/D_1_Pm(theta_b);
f_23_Pm = @(theta_b,d_theta_b,d_phi_v,v) -(R_Pm^2)*sin(theta_b)*(d_phi_v)*(m_b_Pm*z_Pm*v + 2*cos(theta_b)*d_theta_b*(I_xz_Pm+m_b_Pm*(z_Pm^2)))/D_2_Pm(theta_b);
g_15_Pm = @(theta_b) J_a_Pm + R_Pm*cos(theta_b)*z_Pm*m_b_Pm;
g_16_Pm = @(theta_b) -R_Pm*(R_Pm*cos(theta_b)*z_Pm*m_b_Pm+J_b_Pm);

%% データ用配列の用意
%状態変数
%プラント
x_P         = zeros(L,7);
xd_P        = zeros(L,7);
xv_P        = zeros(L,1);
yv_P        = zeros(L,1);
phi_v_P     = zeros(L,1);
theta_b_P   = zeros(L,1);
d_theta_b_P = zeros(L,1);
v_P         = zeros(L,1);
d_phi_v_P   = zeros(L,1);

%モデル
x_Pm         = zeros(L,7);
xd_Pm        = zeros(L,7);
xv_Pm        = zeros(L,1);
yv_Pm        = zeros(L,1);
phi_v_Pm     = zeros(L,1);
theta_b_Pm   = zeros(L,1);
d_theta_b_Pm = zeros(L,1);
v_Pm         = zeros(L,1);
d_phi_v_Pm   = zeros(L,1);

%入力信号
w_1_P = zeros(L,1);
w_2_P = zeros(L,1);
w_1_Pm = zeros(L,1);
w_2_Pm = zeros(L,1);
u_a_P = zeros(L,1);
u_b_P = zeros(L,1);
u_a_Pm = zeros(L,1);
u_b_Pm = zeros(L,1);
tau_wr_P = zeros(L,1);
tau_wl_P = zeros(L,1);
tau_wr_Pm = zeros(L,1);
tau_wl_Pm = zeros(L,1);

%MEC部分
theta_br_dash = zeros(L,1);
ev = zeros(L,1);
ev_i = zeros(L,1);
ev_d = zeros(L,1);
uc = zeros(L,1);

%仮想点
xv_dash_P = zeros(L,1);
yv_dash_P = zeros(L,1);
vx_dash_P = zeros(L,1);
vy_dash_P = zeros(L,1);
v_dash_P = zeros(L,1);

xv_dash_Pm = zeros(L,1);
yv_dash_Pm = zeros(L,1);
vx_dash_Pm = zeros(L,1);
vy_dash_Pm = zeros(L,1);
v_dash_Pm = zeros(L,1);

%指令値
d_v = zeros(L,1);
phi_vr = zeros(L,1);
theta_br = zeros(L,1);

time_data = zeros(L,1);

%% 初期値を与える
xv_P(1,1)        = 0;
yv_P(1,1)        = 0;
phi_v_P(1,1)     = 0;
theta_b_P(1,1)   = 0;
d_theta_b_P(1,1) = 0;
v_P(1,1)         = 0;
d_phi_v_P(1,1)   = 0;

x_P(1,1) = xv_P(1,1);
x_P(1,2) = yv_P(1,1);
x_P(1,3) = phi_v_P(1,1);
x_P(1,4) = theta_b_P(1,1);
x_P(1,5) = d_theta_b_P(1,1);
x_P(1,6) = v_P(1,1);
x_P(1,7) = d_phi_v_P(1,1);

%モデル
xv_Pm(1,1)        = 0;
yv_Pm(1,1)        = 0;
phi_v_Pm(1,1)     = 0;
theta_b_Pm(1,1)   = 0;
d_theta_b_Pm(1,1) = 0;
v_Pm(1,1)         = 0;
d_phi_v_Pm(1,1)   = 0;
    
x_Pm(1,1) = xv_Pm(1,1);
x_Pm(1,2) = yv_Pm(1,1);
x_Pm(1,3) = phi_v_Pm(1,1);
x_Pm(1,4) = theta_b_Pm(1,1);
x_Pm(1,5) = d_theta_b_Pm(1,1);
x_Pm(1,6) = v_Pm(1,1);
x_Pm(1,7) = d_phi_v_Pm(1,1);


%%
for i = 1:1:L-1
%% 指令値

    %一定速度で走行
    time_data(i,1) = i * Ts;
    time = time_data(i,1);
    
%     %　5[deg]傾ける
%     if time>=2.0 && time<2.5
%         d_v(i,1) = 0.8573;     %加速度指令値
%         phi_vr(i,1) = 0;     %旋回角度指令
%     elseif time>=2.5 && time<3.0
%         d_v(i,1) = -0.8573;     %加速度指令値
%         phi_vr(i,1) = 0;     %旋回角度指令
%     else
%         d_v(i,1) = 0;
%         phi_vr(i,1) = 0;
%     end

%     %　円旋回2
%     if time>=0.0 && time<2.0
%         d_v(i,1) = 0.0;     %加速度指令値
%         phi_vr(i,1) = 0;     %旋回角度指令
%     else 
%         d_v(i,1) = d_v(i-1,1) + 0.001;
%         phi_vr(i,1) = phi_vr(i-1,1) + 1.0*(pi/180);
%     end

    if time>=0.0 && time<=3.0
        d_v(i,1) = 0.0;
        phi_vr(i,1) = 0;
        
    elseif time>=3.0 && time<=5.0
        d_v(i,1) = 0.188;
        phi_vr(i,1) = 0;
        
    elseif time>=5.0 && time<=8.0
        d_v(i,1) = 0;
        phi_vr(i,1) = phi_vr(i-1,1) + 90.0*(pi/180)*Ts;
        
        if phi_vr(i,1) >= pi
          phi_vr(i,1) = pi;
        end
        
    elseif time>=8.0 && time<=11.0
        d_v(i,1) = 0;

        phi_vr(i,1) = phi_vr(i-1,1) - 90.0*(pi/180)*Ts;
        
        if phi_vr(i,1) <= 0
          phi_vr(i,1) = 0;
        end
        
    elseif time>=11.0 && time <=13.0
        d_v(i,1) = -0.188;    
    else
        d_v(i,1) = 0;
        phi_vr(i,1) = 0;
    end
    
%% 加速度指令値から目標振子角度を算出
    theta_br(i,1) = (asin(d_v(i,1)/(sqrt(g^2 + d_v(i,1)^2)))) - (asin( -d_v(i,1)*(m_b_Pm*R_Pm^2 + 2*I_wa_Pm + 2*m_w_Pm*R_Pm^2)/m_b_Pm*R_Pm*z_Pm*sqrt(d_v(i,1)^2 + g^2)));

%% MECの補償器部分

    %vを用いた場合
    Kp = 2.0;
    Ki = 0.0;
    Kd = 0.075;
    
    if i>=2
%       ev(i,1) = v_P(i,1) - v_Pm(i,1);
      ev(i,1) = v_dash_P(i,1) - v_dash_Pm(i,1);
      ev_d(i,1) = (ev(i,1) - ev(i-1))/Ts;
      ev_i(i,1) = ev_i(i-1,1) + ev(i,1);
    end
    
    uc(i,1) = Kp*ev(i,1) + Ki*ev_i(i,1) + Kd*ev_d(i,1);
%     uc(i,1) = 0; 

    %補償入力
    theta_br_dash(i,1) = theta_br(i,1) - uc(i,1);

%% 線形化フィードバックに伴う新しい制御入力の算出
    %フィードバックゲイン
    %シミュレーション用
    f_1_Pm = 300.0;
    f_2_Pm = 35.0;
    f_3_Pm = 300.0;
    f_4_Pm = 30.0;
    
    f_1_P = 300.0;
    f_2_P = 35.0;
    f_3_P = 300.0;
    f_4_P = 30.0;

    w_1_Pm(i,1) = -f_1_Pm*(phi_v_Pm(i,1) - phi_vr(i,1)) - f_2_Pm*d_phi_v_Pm(i,1);       %dd_phi_v
    w_2_Pm(i,1) = -f_3_Pm*(theta_b_Pm(i,1) - theta_br(i,1)) - f_4_Pm*d_theta_b_Pm(i,1); %dd_theta_v
    
    w_1_P(i,1) = -f_1_P*(phi_v_P(i,1) - phi_vr(i,1)) - f_2_P*d_phi_v_P(i,1);       %dd_phi_v
    w_2_P(i,1) = -f_3_P*(theta_b_P(i,1) - theta_br_dash(i,1)) - f_4_P*d_theta_b_P(i,1); %dd_theta_v
    
%% 線形化フィードバックのための見かけ上の制御入力の定義
    u_a_Pm(i,1) = w_2_Pm(i,1)/g_15_Pm(theta_b_Pm(i,1)) - f_21_Pm(theta_b_Pm(i,1),d_theta_b_Pm(i,1),d_phi_v_Pm(i,1))/g_15_Pm(theta_b_Pm(i,1));
    u_b_Pm(i,1) = w_1_Pm(i,1) - f_23_Pm(theta_b_Pm(i,1),d_theta_b_Pm(i,1),d_phi_v_Pm(i,1),v_Pm(i,1));
    
    u_a_P(i,1) = w_2_P(i,1)/g_15_Pm(theta_b_P(i,1)) - f_21_Pm(theta_b_P(i,1),d_theta_b_P(i,1),d_phi_v_P(i,1))/g_15_Pm(theta_b_P(i,1));
    u_b_P(i,1) = w_1_P(i,1) - f_23_Pm(theta_b_P(i,1),d_theta_b_P(i,1),d_phi_v_P(i,1),v_P(i,1));

%% 見かけ上の入力uを左右輪のトルクtauに変換
    tau_wr_Pm(i,1) = D_1_Pm(theta_b_Pm(i,1))/2*u_a_Pm(i,1) + D_2_Pm(theta_b_Pm(i,1))/(2*R_Pm*b_Pm)*u_b_Pm(i,1);%右車輪の入力トルク
    tau_wl_Pm(i,1) = D_1_Pm(theta_b_Pm(i,1))/2*u_a_Pm(i,1) - D_2_Pm(theta_b_Pm(i,1))/(2*R_Pm*b_Pm)*u_b_Pm(i,1);%左車輪の入力トルク
    
    tau_wr_P(i,1) = D_1_Pm(theta_b_P(i,1))/2*u_a_P(i,1) + D_2_Pm(theta_b_P(i,1))/(2*R_Pm*b_Pm)*u_b_P(i,1);%右車輪の入力トルク
    tau_wl_P(i,1) = D_1_Pm(theta_b_P(i,1))/2*u_a_P(i,1) - D_2_Pm(theta_b_P(i,1))/(2*R_Pm*b_Pm)*u_b_P(i,1);%左車輪の入力トルク
    
%% 平行2輪型倒立振子の状態方程式
    %モデル
    %微分方程式
    xd_Pm(i,1) = cos(phi_v_Pm(i,1))*v_Pm(i,1);
    xd_Pm(i,2) = sin(phi_v_Pm(i,1))*v_Pm(i,1);
    xd_Pm(i,3) = d_phi_v_Pm(i,1);
    xd_Pm(i,4) = d_theta_b_Pm(i,1);
    xd_Pm(i,5) = f_21_Pm(theta_b_Pm(i,1),d_theta_b_Pm(i,1),d_phi_v_Pm(i,1)) + (g_15_Pm(theta_b_Pm(i,1))/D_1_Pm(theta_b_Pm(i,1)))*tau_wr_Pm(i,1) + (g_15_Pm(theta_b_Pm(i,1))/D_1_Pm(theta_b_Pm(i,1)))*tau_wl_Pm(i,1);
    xd_Pm(i,6) = f_22_Pm(theta_b_Pm(i,1),d_theta_b_Pm(i,1),d_phi_v_Pm(i,1)) + (g_16_Pm(theta_b_Pm(i,1))/D_1_Pm(theta_b_Pm(i,1)))*tau_wr_Pm(i,1) + (g_16_Pm(theta_b_Pm(i,1))/D_1_Pm(theta_b_Pm(i,1)))*tau_wl_Pm(i,1);
    xd_Pm(i,7) = f_23_Pm(theta_b_Pm(i,1),d_theta_b_Pm(i,1),d_phi_v_Pm(i,1),v_Pm(i,1)) + R_Pm*b_Pm/D_2_Pm(theta_b_Pm(i,1))*tau_wr_Pm(i,1) - R_Pm*b_Pm/D_2_Pm(theta_b_Pm(i,1))*tau_wl_Pm(i,1);
    %出力方程式
    x_Pm(i+1,1) = x_Pm(i,1) + xd_Pm(i,1)*Ts;
    x_Pm(i+1,2) = x_Pm(i,2) + xd_Pm(i,2)*Ts;
    x_Pm(i+1,3) = x_Pm(i,3) + xd_Pm(i,3)*Ts;
    x_Pm(i+1,4) = x_Pm(i,4) + xd_Pm(i,4)*Ts;
    x_Pm(i+1,5) = x_Pm(i,5) + xd_Pm(i,5)*Ts;
    x_Pm(i+1,6) = x_Pm(i,6) + xd_Pm(i,6)*Ts;
    x_Pm(i+1,7) = x_Pm(i,7) + xd_Pm(i,7)*Ts;
    
    xv_Pm(i+1,1)        = x_Pm(i+1,1);
    yv_Pm(i+1,1)        = x_Pm(i+1,2);
    phi_v_Pm(i+1,1)     = x_Pm(i+1,3);
    theta_b_Pm(i+1,1)   = x_Pm(i+1,4);
    d_theta_b_Pm(i+1,1) = x_Pm(i+1,5);
    v_Pm(i+1,1)         = x_Pm(i+1,6);
    d_phi_v_Pm(i+1,1)   = x_Pm(i+1,7);
    
    xv_dash_Pm(i+1,1) = xv_Pm(i+1,1) + L_body * sin(theta_b_Pm(i+1,1)) * cos(phi_v_Pm(i+1,1));
    yv_dash_Pm(i+1,1) = yv_Pm(i+1,1) + L_body * sin(theta_b_Pm(i+1,1)) * sin(phi_v_Pm(i+1,1));
    
    vx_dash_Pm(i+1,1) = ( xv_dash_Pm(i+1,1) - xv_dash_Pm(i,1) ) / Ts;
    vy_dash_Pm(i+1,1) = ( yv_dash_Pm(i+1,1) - yv_dash_Pm(i,1) ) / Ts;
    v_dash_Pm(i+1,1)  = vx_dash_Pm(i+1,1) * cos(phi_v_Pm(i+1,1)) + vy_dash_Pm(i+1,1) * sin(phi_v_Pm(i+1,1));
    
    %プラント
    %微分方程式
    xd_P(i,1) = cos(phi_v_P(i,1))*v_P(i,1);
    xd_P(i,2) = sin(phi_v_P(i,1))*v_P(i,1);
    xd_P(i,3) = d_phi_v_P(i,1);
    xd_P(i,4) = d_theta_b_P(i,1);
    xd_P(i,5) = f_21_P(theta_b_P(i,1),d_theta_b_P(i,1),d_phi_v_P(i,1)) + (g_15_P(theta_b_P(i,1))/D_1_P(theta_b_P(i,1)))*tau_wr_P(i,1) + (g_15_P(theta_b_P(i,1))/D_1_P(theta_b_P(i,1)))*tau_wl_P(i,1);
    xd_P(i,6) = f_22_P(theta_b_P(i,1),d_theta_b_P(i,1),d_phi_v_P(i,1)) + g_16_P(theta_b_P(i,1))/D_1_P(theta_b_P(i,1))*tau_wr_P(i,1) + g_16_P(theta_b_P(i,1))/D_1_P(theta_b_P(i,1))*tau_wl_P(i,1);
    xd_P(i,7) = f_23_P(theta_b_P(i,1),d_theta_b_P(i,1),d_phi_v_P(i,1),v_P(i,1)) + R_P*b_P/D_2_P(theta_b_P(i,1))*tau_wr_P(i,1) - R_P*b_P/D_2_P(theta_b_P(i,1))*tau_wl_P(i,1);
    %出力方程式
    x_P(i+1,1) = x_P(i,1) + xd_P(i,1)*Ts;
    x_P(i+1,2) = x_P(i,2) + xd_P(i,2)*Ts;
    x_P(i+1,3) = x_P(i,3) + xd_P(i,3)*Ts;
    x_P(i+1,4) = x_P(i,4) + xd_P(i,4)*Ts;
    x_P(i+1,5) = x_P(i,5) + xd_P(i,5)*Ts;
    x_P(i+1,6) = x_P(i,6) + xd_P(i,6)*Ts;
    x_P(i+1,7) = x_P(i,7) + xd_P(i,7)*Ts; 
    
    xv_P(i+1,1)        = x_P(i+1,1);
    yv_P(i+1,1)        = x_P(i+1,2);
    phi_v_P(i+1,1)     = x_P(i+1,3);
    theta_b_P(i+1,1)   = x_P(i+1,4);
    d_theta_b_P(i+1,1) = x_P(i+1,5);
    v_P(i+1,1)         = x_P(i+1,6);
    d_phi_v_P(i+1,1)   = x_P(i+1,7);
    
    xv_dash_P(i+1,1) = xv_P(i+1,1) + L_body * sin(theta_b_P(i+1,1)) * cos(phi_v_P(i+1,1));
    yv_dash_P(i+1,1) = yv_P(i+1,1) + L_body * sin(theta_b_P(i+1,1)) * sin(phi_v_P(i+1,1));
    
    vx_dash_P(i+1,1) = ( xv_dash_P(i+1,1) - xv_dash_P(i,1) ) / Ts;
    vy_dash_P(i+1,1) = ( yv_dash_P(i+1,1) - yv_dash_P(i,1) ) / Ts;
    v_dash_P(i+1,1)  = vx_dash_P(i+1,1) * cos(phi_v_P(i+1,1)) + vy_dash_P(i+1,1) * sin(phi_v_P(i+1,1));
    
%    %平衡点から10degずれた車両における仮想観測点の速度
%     theta_b_P2(i+1,1) = theta_b_P(i+1,1) - 10.0 * (pi/180);
%     xv_dash_P2(i+1,1) = xv_P(i+1,1) + L_body * sin(theta_b_P2(i+1,1)) * cos(phi_v_P(i+1,1));
%     yv_dash_P2(i+1,1) = yv_P(i+1,1) + L_body * sin(theta_b_P2(i+1,1)) * sin(phi_v_P(i+1,1));
%     
%     vx_dash_P2(i+1,1) = ( xv_dash_P2(i+1,1) - xv_dash_P2(i,1) ) / Ts;
%     vy_dash_P2(i+1,1) = ( yv_dash_P2(i+1,1) - yv_dash_P2(i,1) ) / Ts;
%     v_dash_P2(i+1,1)  = vx_dash_P2(i+1,1) * cos(phi_v_P(i+1,1)) + vy_dash_P2(i+1,1) * sin(phi_v_P(i+1,1));
%     v_dash_P2(2,1) = 0;
    
end

run('plot_fig_MEC');

% figure(100)%仮想点の速度
% hold on 
% grid on
% plot(t,v_dash_P2,'g')
% plot(t,v_dash_Pm,'k--')
% xlabel('time[s]','FontSize',16);
% ylabel('v dash[m/s]','FontSize',16);
% % legend('MEC','reference(model)','Location','SouthEast');


end
