%%%%%%%%         ステップ応答         %%%%%%%
%%%%%%%%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=0;
Ki=0;
Kd=0;

%P制御用
Kp1=2.75;
Kp2=5.75;
Kp3=9.75;


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

cnum_pid2=[Kd Kp2 Ki];

cnum_pid3=[Kd Kp3 Ki];


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

num_pid2=conv(pnum,cnum_pid2);
den_pid=conv(pden,cden_pid);
den_pid2=den_pid+num_pid2;
pole_pid2=roots(den_pid)

num_pid3=conv(pnum,cnum_pid3);
den_pid=conv(pden,cden_pid);
den_pid3=den_pid+num_pid3;
pole_pid3=roots(den_pid)


%%%%%%%%ステップ応答コマンド
y_pid1=step(num_pid1,den_pid1,t);
y_pid2=step(num_pid2,den_pid2,t);
y_pid3=step(num_pid3,den_pid3,t);
y_plant=step(pnum,pden,t);

%%%%%%%%%グラフのプロット
figure(1)
%subplot(211)
plot(t,y_plant,'k-',t,y_pid1,'r:',t,y_pid2,'g--',t,y_pid3,'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 : Kp=2.75');
  tx3=3.6;
  ty3=0.35;
  text(tx3,ty3,'- -green : Kp=5.75');
  tx4=3.6;
  ty4=0.25;
  text(tx4,ty4,'-*-blue : Kp=9.75');
xlabel('Time(sec)'),ylabel('x')
title('Step response (Kp)')

