%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% ISCIE 56-4 2012
% 初学者のための図解でわかる制御工学I
% サーボ系・内部モデル原理（MATLABファイル）
% 第2章数値例
% 目標信号：ステップ
% 外乱信号：ステップ
% -------------------------------------
% | 外乱無し(P制御)  | cart_p.mdl      | 
% | 外乱有り(P制御)  | cart_p_dis.mdl  |
% | 外乱有り(PI制御) | cart_pi_dis.mdl |
% -------------------------------------
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

close all;
clear all;

%% 台車のパラメータ

%----------------------------------
% シミュレーションパラメータ
x0 = [0;0]; % 台車の初期値
            % 位置，速度
time = 5; %シミュレーション時間
r0 = 0.5; %ステップ目標信号の大きさ[m]
d0 = 3; %ステップ外乱信号の大きさ[v]
st_time=0; %ステップ信号の開始時間
% グラフ表示パラメータ
ymin = 0;
ymax = r0*1.1;
% 制御器のパラメータ
kp=20; %比例ゲイン
ki=15; %積分ゲイン

%----------------------------------
%第1表台車の物理定数
mc = 0.6; %台車の質量
muc = 2.5; %台車の粘性摩擦の比例定数
alpha = 0.3; %モータ電圧から台車の駆動力へのゲイン
g = 9.80; % 重力加速度

%% 台車の制御（2章）

%----------------------------------
% 比例制御シミュレーション(外乱なし)
sim('cart_p',time); 
% 比例制御シミュレーション(外乱あり)
sim('cart_p_dis',time); 
% PI制御シミュレーション(外乱あり)
sim('cart_pi_dis',time); 

figure(1)
    hold on;
    grid on;
    box on;
    plot(r.time,r.signals.values,'k-','linewidth',1);
    plot(yp.time,yp.signals.values,'k:','linewidth',2);    
    plot(ypd.time,ypd.signals.values,'k--','linewidth',2);  
    plot(ypid.time,ypid.signals.values,'k-','linewidth',2);
    set(gca,'Ylim',[0 0.9]);
    set(gca,'Xlim',[0 time]);
    xlabel('time t [s]','FontSize',15);
    ylabel('z(t) and r(t)','FontSize',15);   
    legend('r_0=0.5','P制御（外乱無）','P制御（外乱有）','PI制御（外乱有）','FontSize',12)