%%%%%%%%         ステップ応答         %%%%%%%
%%%%%%%%PID,PD,PI Controllerによる制御%%%%%%%
clear;	%初期化

t=0:0.01:15;	%時間軸(0～5秒、0.01刻み)

%プラントの定義
m=0.1;		%質量m
tuta=0.5392;	%減衰比ζ
w=3.8623;	%固有振動数ω

k=m*w^2;	%バネ定数k
c=m*2*w*tuta;	%減衰係数c

%%%%%%%%プラントの伝達関数表現
pnum=[0 0 1/m];
pden=[1 c/m k/m];
pole_plant=roots(pden)


%%%%%%%%コントローラの各ゲイン
Kp=9.95;
Ki=7.6343;
Kd=1.6513;

%P・I制御用
%Kp=2.75;
%Ki=2.5;


%%%%%%%%各コントローラの伝達関数
%ＰＩＤコントローラの伝達関数表現
%PID Controller==Kp+Ki/s+Kd*s==(Kp*s+Ki+Kd*s^2)/s
cnum_pid=[Kd Kp Ki];
cden_pid=[0 1 0];

%ＰＤコントローラの伝達関数表現
%PD Controller==Kp+Kd*s
cnum_pd=[0 Kd Kp];
cden_pd=[0 0 1];

%ＰＩコントローラの伝達関数表現
%PI Controller==Kp+Ki/s==(Kp*s+Ki)/s
cnum_pi=[0 Kp Ki];
cden_pi=[0 1 0];


%%%%%%%%システム全体の伝達関数表現 W=PC/(1+PC)
num_pid=conv(pnum,cnum_pid);
den_pid=conv(pden,cden_pid);
den_pid=den_pid+num_pid;
pole_pid=roots(den_pid)

num_pd=conv(pnum,cnum_pd);
den_pd=conv(pden,cden_pd);
den_pd=den_pd+num_pd;

num_pi=conv(pnum,cnum_pi);
den_pi=conv(pden,cden_pi);
den_pi=den_pi+num_pi;


%%%%%%%%ステップ応答コマンド
y_pid=step(num_pid,den_pid,t);
y_pd=step(num_pd,den_pd,t);
y_pi=step(num_pi,den_pi,t);
y_plant=step(pnum,pden,t);

%%%%%%%%%グラフのプロット
figure(1)
%subplot(211)
plot(t,y_plant,'k',t,y_pid,'r',t,y_pd,'g',t,y_pi,'b')
axis([0,5,0,1.2])%x軸y軸の最大最小
grid
  tx1=3.6;
  ty1=0.55;
  text(tx1,ty1,'black : Plant');
  tx2=3.6;
  ty2=0.45;
  text(tx2,ty2,'red : PID');
  tx3=3.6;
  ty3=0.35;
  text(tx3,ty3,'green : PD');
  tx4=3.6;
  ty4=0.25;
  text(tx4,ty4,'blue : PI');
xlabel('Time(sec)'),ylabel('x')
title('Step response (PID,PD,PI Control)')

