close all;
echo on;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 以下では，位相遅れ (PI) 補償器を構成していきます．
% はじめに，p01 において決定したゲイン補償 k による
% 制御系の応答を表示します．
% つぎに, 表示された応答をもとに，適切な折点角周波数 1/t を設定し，
% パラメータ t の値を決定してください．
% このとき，位相余裕が劣化しないように注意してください．
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

pause % press any key to continue

% --------------------------------------------------------------------
% 決定したゲイン補償 k によるボード線図とステップ応答を表示します．
% なお，ボード線図には，ゲイン補償 k も同時にプロットします．
% --------------------------------------------------------------------

%%%%% 決定したゲイン補償 k を入力してください．
k = input('k = ');
p = tf([20],[1 11 10]);
c1 = k;
l1 = p*c1;

%%%%% ゲイン補償による閉ループ系は次で与えられます．
t1 = feedback(l1,1);

%%%%% l1 bode 線図のデータ
[magl1,phasel1,wl1]=bode(l1,{1e-2,1e2});
[gml1,pml1,wpcl1,wgcl1] = margin(l1);
logmagl1 = 20*log10(arr2vec(magl1,wl1));
phl1 = arr2vec(phasel1,wl1);

%%%%% 閉ループ t ステップ応答のデータ
[yt1,tt1] = lsim(t1,uu,time);

%%%%%%%%%%%%%%% p 補償 bode 線図のプロット
fig01 = figure(1); clf;
%set(fig01,'position',[20 20 460 650]);
set(fig01,'position',[20 140 560 750]);

%%%%% gain 線図の axes
hgain = axes('position',[0.2 0.65 0.75 0.34],...
	     'box','on',...
	     'fontName','Helvetica',...
	     'fontsize',14,...
	     'xscale','log',...
	     'xlim',[1e-2 1e2],...
	     'xtick',[1e-2 1e-1 1e0 1e1 1e2],...
	     'xgrid','off',...
	     'ylim',[-30 50],...
	     'ytick',[-20 0 20 40],...
	     'ygrid','on',...
	     'gridlinestyle',':');
set(fig01,'currentaxes',hgain); %%%%% x 軸のグリッドを書いておく
hold on; plot([1e-1 1e-1],[100 -100],':k',[1e-0 1e-0],[100 -100],':k',...
	      [1e1 1e1],[100 -100],':k');
set(get(hgain,'xlabel'),...
    'string','\omega[rad/s]',...
    'fontname','helvetica',...
    'fontsize',14); %%%%% x 軸ラベル 
set(get(hgain,'ylabel'),...
    'string','gain [dB]',...
    'fontname','helvetica',...
    'fontsize',14); %%%%% y 軸ラベル
set(fig01,'currentaxes',hgain); %%%%% 0 [dB] の線 
hold on; l0gain = plot([1e-2 1e2],[0 0],'-.b');

%%%%% 位相線図の axes
hphase = axes('position',[0.2 0.15 0.75 0.34],...
	      'box','on',...
	      'fontname','Helvetica',...
	      'fontsize',14,...
	      'xscale','log',...
	      'xlim',[1e-2 1e2],...
	      'xtick',[1e-2 1e-1 1e0 1e1 1e2],...
	      'xgrid','off',...
	      'ylim',[-200 0],...
	      'ytick',[-180 -135 -90 -45 0],...
	      'ygrid','on',...
	      'gridlinestyle',':');
set(fig01,'currentaxes',hphase); %%%%% x 軸のグリッドを書いておく
hold on; plot([1e-1 1e-1],[-200 0],':k',[1e-0 1e-0],[-200 0],':k',...
	      [1e1 1e1],[-200 0],':k');
set(get(hphase,'xlabel'),'string','\omega[rad/s]',...
		  'fontname','helvetica',...
		  'fontsize',14); %%%%% x 軸ラベル
set(get(hphase,'ylabel'),'string','phase [deg]',...
		  'fontname','helvetica',...
		  'fontsize',14); %%%%% y 軸ラベル
set(fig01,'currentaxes',hphase); %%%%% -180 [deg] の線 
hold on; lm180phase = plot([1e-2 1e2],[-180 -180],'-.b');

%%%%% コメント用の axes
hcommentb = axes('position',[0 0 1 1],...
		'visible','off',...
		'xlim',[0 1],...
		'ylim',[0 1]);

%%%%% gain 線図のプロット %%%%% P
set(fig01,'currentaxes',hgain);
hold on; plogmagp = plot(wp,logmagp,'-c'); set(plogmagp,'linewidth',1);
hold on; pwgp = plot([wgcp wgcp],[0 -30],'-.b');

%%%%% gain 線図のプロット %%%%% C
set(fig01,'currentaxes',hgain);
hold on; plot([10^-2 10^2],[20 20],'g');

%%%%% gain 線図のプロット %%%%% L1
set(fig01,'currentaxes',hgain);
hold on; plogmagl1 = plot(wl1,logmagl1,'-m'); set(plogmagl1,'linewidth',2);
hold on; pwgp = plot([wgcl1 wgcl1],[0 -30],'-.b');

%%%%% 位相線図のプロット %%%%% P
set(fig01,'currentaxes',hphase);
hold on; pphp = plot(wp,php,'-c'); set(pphp,'linewidth',1);
hold on; pwgcp = plot([wgcp wgcp],[-180 0],'-.b');
hold on; ppmp = plot([wgcp wgcp],[-180+pmp -180],'-r');
set(ppmp,'linewidth',1);

%%%%% 位相線図のプロット %%%%% L1
set(fig01,'currentaxes',hphase);
hold on; pphl1 = plot(wl1,phl1,'-m'); set(pphl1,'linewidth',2);
hold on; pwgcl1 = plot([wgcl1 wgcl1],[-180 0],'-.b');
hold on; ppml1 = plot([wgcl1 wgcl1],[-180+pml1 -180],'-r');
set(ppml1,'linewidth',2);

%%%%% コメント
set(fig01,'currentaxes',hcommentb);
tgain = text(0.5,0.55,'(a) gain',...
	     'fontname','helvetica',...
	     'fontsize',14);
tgain = text(0.5,0.05,'(b) phase',...
	     'fontname','helvetica',...
	     'fontsize',14);
swgcl1 = {sprintf('\\omega_{gc}=%2.1f',wgcl1)};
text(0.77,0.85,swgcl1,...
     'FontName','Times',...
     'FontSize',14,...
     'Color','b');
swgcl1d = {sprintf('gain compensation k=%2.1f',k)};
text(0.47,0.93,swgcl1d,...
     'FontName','Times',...
     'FontSize',14,...
     'Color','b');

%%%%%%%%%% p 補償ステップ応答のプロット
fig02 = figure(2); clf;
% set(fig02,'position',[500 0 460 670]);
set(fig02,'position',[600 140 560 750]);

%%%%% ステップ応答の axes
houtput = axes('position',[0.2 0.65 0.75 0.34],...
	     'box','on',...
	     'fontName','Helvetica',...
	     'fontsize',14,...
	     'xscale','linear',...
	     'xlim',[0 1.5],...
	     'xtick',[0 0.5 1 1.5],...
	     'xgrid','on',...
	     'ylim',[0 1.4],...
	     'ytick',[0 0.5 1],...
	     'ygrid','on',...
	     'gridlinestyle',':');
set(get(houtput,'xlabel'),...
    'string','\itt \rm[s]',...
    'fontname','helvetica',...
    'fontsize',14); %%%%% x 軸ラベル 
set(get(houtput,'ylabel'),...
    'string','\ity\rm(\itt\rm)',...
    'fontname','helvetica',...
    'fontsize',14); %%%%% y 軸ラベル

%%%%% 入力の axes
%%%%% コメント用の axes
hcomment = axes('position',[0 0 1 1],...
		'visible','off',...
		'xlim',[0 1],...
		'ylim',[0 1]);

%%%%% 出力 y のプロット P
set(fig02,'currentaxes',houtput);
hold on; pyt = plot(tt,yt,'-c'); set(pyt,'linewidth',1);

%%%%% 出力 y のプロット L1
set(fig02,'currentaxes',houtput);
hold on; pyt1 = plot(tt1,yt1,'-m'); set(pyt1,'linewidth',2);

%%%%% コメント
set(fig02,'currentaxes',hcomment);
tgain = text(0.5,0.55,'(a) output \ity\rm(\itt\rm)',...
	     'fontname','helvetica',...
	     'fontsize',14);
%%%%%
swgcl1d = {sprintf('gain compensation k=%2.1f',k)};
text(0.5,0.93,swgcl1d,...
     'FontName','Times',...
     'FontSize',14,...
     'Color','b');

%%%%% コメント用の axes
hcomment = axes('position',[0 0 1 1],...
		'visible','off',...
		'xlim',[0 1],...
		'ylim',[0 1]);

%%%%% 出力 y のプロット %%%%% P
set(fig02,'currentaxes',houtput);
hold on; pyt = plot(tt,yt,'-c'); set(pyt,'linewidth',1);

%%%%% p 補償ステップ応答のプロット %%%%% L1
set(fig02,'currentaxes',houtput);
hold on; pyt1 = plot(tt,yt1,'-m'); set(pyt1,'linewidth',2);

pause % press any key to continue

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% ゲイン補償 k によるボード線図とステップ応答が表示されました．
% 水色の線が補償前，ピンク色の線が補償後の応答を表しています．
% また，緑色の線は，ゲイン補償器 (コントローラ) のみの応答を表しています．
%
% ここで，位相線図では，ゲイン補償前と補償後の特性が変化しないことに注意
% してください (Figure 1 では，補償前の応答と補償後の応答が重なっています)．
%
% なお，ゲイン補償のみでは，位相余裕を確保することを前提として，定常特性
% (定常偏差) の改善には限界があることに注意してください．
%
% この手順は, 講義スライド 13 か テキスト P.159 の
% 位相遅れ補償設計手順の [ステップ2] です.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 次に，位相遅れ補償器の折点角周波数 1/t を決定します．
% ゲイン補償によるボード線図 (Figure 1) を参照しながら，位相遅れに注意して
% 折点角周波数 1/t を定め，パラメータ t の値を決定してください．
%
% この手順は, 講義スライド 13 か テキスト P.159 の
% 位相遅れ補償設計手順の [ステップ4] です．
% (今回の課題において [ステップ3] は行いません)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%% 決定したパラメータ t を入力してください．
t = input('t = ');

pause % press any key to continue

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% これより，位相遅れ (PI) 補償器，位相遅れ補償による開ループ伝達関数，
% および閉ループ伝達関数は次式で与えられます．
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%% 位相遅れ (PI) 補償器は，次式で与えられます．
c2 = tf([k*t k],[t 0]);
zpk(c2)

pause % press any key to continue
%%% 位相遅れ補償による開ループ伝達関数は，次式で与えられます．
l2 = p*c2;
zpk(l2)

pause % press any key to continue

%%%%% 位相遅れ補償による閉ループ伝達関数は，次式で与えられます．
t2 = feedback(l2,1);
zpk(t2)

pause % press any key to continue

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 以上より，構成した位相遅れ補償によるボード線図とステップ応答を表示します．
% なお，ボード線図には，位相遅れ補償器の応答も同時にプロットします．
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

pause % press any key to continue

%%%%% controller bode 線図のデータ
[magc,phasec,wc]=bode(c2,{1e-2,1e2});
[gmc,pmc,wpcc,wgcc] = margin(c2);
logmagc = 20*log10(arr2vec(magc,wc));
phc = arr2vec(phasec,wc);

%%%%% l2 bode 線図のデータ
[magl2,phasel2,wl2] = bode(l2,{1e-2,1e2});
[gml2,pml2,wpcl2,wgcl2] = margin(l2);
logmagl2 = 20*log10(arr2vec(magl2,wl2));
phl2 = arr2vec(phasel2,wl2);

%%%%% 閉ループ t のステップ応答のデータ
[yt2,tt2] = lsim(t2,uu,time);

%%%%%%%%%%%%%%% PI 補償 bode 線図のプロット
fig01 = figure(1); clf;
set(fig01,'position',[20 140 560 750]);

%%%%% gain 線図の axes
hgain = axes('position',[0.2 0.65 0.75 0.34],...
	     'box','on',...
	     'fontName','Helvetica',...
	     'fontsize',14,...
	     'xscale','log',...
	     'xlim',[1e-2 1e2],...
	     'xtick',[1e-2 1e-1 1e0 1e1 1e2],...
	     'xgrid','off',...
	     'ylim',[-30 50],...
	     'ytick',[-20 0 20 40],...
	     'ygrid','on',...
	     'gridlinestyle',':');
set(fig01,'currentaxes',hgain); %%%%% x 軸のグリッドを書いておく
hold on; plot([1e-1 1e-1],[100 -100],':k',[1e-0 1e-0],[100 -100],':k',...
	      [1e1 1e1],[100 -100],':k');
set(get(hgain,'xlabel'),...
    'string','\omega[rad/s]',...
    'fontname','helvetica',...
    'fontsize',14); %%%%% x 軸ラベル 
set(get(hgain,'ylabel'),...
    'string','gain [dB]',...
    'fontname','helvetica',...
    'fontsize',14); %%%%% y 軸ラベル
set(fig01,'currentaxes',hgain); %%%%% 0 [dB] の線 
hold on; l0gain = plot([1e-2 1e2],[0 0],'-.b');

%%%%% 位相線図の axes
hphase = axes('position',[0.2 0.15 0.75 0.34],...
	      'box','on',...
	      'fontname','Helvetica',...
	      'fontsize',14,...
	      'xscale','log',...
	      'xlim',[1e-2 1e2],...
	      'xtick',[1e-2 1e-1 1e0 1e1 1e2],...
	      'xgrid','off',...
	      'ylim',[-200 0],...
	      'ytick',[-180 -135 -90 -45 0],...
	      'ygrid','on',...
	      'gridlinestyle',':');
set(fig01,'currentaxes',hphase); %%%%% x 軸のグリッドを書いておく
hold on; plot([1e-1 1e-1],[-200 0],':k',[1e-0 1e-0],[-200 0],':k',...
	      [1e1 1e1],[-200 0],':k');
set(get(hphase,'xlabel'),'string','\omega[rad/s]',...
		  'fontname','helvetica',...
		  'fontsize',14); %%%%% x 軸ラベル
set(get(hphase,'ylabel'),'string','phase [deg]',...
		  'fontname','helvetica',...
		  'fontsize',14); %%%%% y 軸ラベル
set(fig01,'currentaxes',hphase); %%%%% -180 [deg] の線 
hold on; lm180phase = plot([1e-2 1e2],[-180 -180],'-.b');

%%%%% コメント用の axes
hcomment = axes('position',[0 0 1 1],...
		'visible','off',...
		'xlim',[0 1],...
		'ylim',[0 1]);

%%%%% gain 線図のプロット %%%%% P
set(fig01,'currentaxes',hgain);
hold on; plogmagp = plot(wp,logmagp,'-c'); set(plogmagp,'linewidth',1);
hold on; pwgp = plot([wgcp wgcp],[0 -30],'-.b');

%%%%% gain 線図のプロット %%%%% C
set(fig01,'currentaxes',hgain);
hold on; plogmagc = plot(wc,logmagc,'-g'); set(plogmagc,'linewidth',1);
hold on; pwgc = plot([wgcc wgcc],[0 -30],'-.g');

%%%%% gain 線図のプロット %%%%% L1
set(fig01,'currentaxes',hgain);
hold on; plogmagl1 = plot(wl1,logmagl1,'-m'); set(plogmagl1,'linewidth',1);
hold on; pwgp = plot([wgcl1 wgcl1],[0 -30],'-.b');

%%%%% gain 線図のプロット %%%%% L2
set(fig01,'currentaxes',hgain);
hold on; plogmagl2 = plot(wl2,logmagl2,'-b'); set(plogmagl2,'linewidth',2);
hold on; pwgp = plot([wgcl2 wgcl2],[0 -30],'-.b');

%%%%% 位相線図のプロット %%%%% P
set(fig01,'currentaxes',hphase);
hold on; pphp = plot(wp,php,'-c'); set(pphp,'linewidth',1);
hold on; pwgcp = plot([wgcp wgcp],[-180 0],'-.b');
hold on; ppmp = plot([wgcp wgcp],[-180+pmp -180],'-r');
set(ppmp,'linewidth',1);

%%%%% 位相線図のプロット %%%%% C
set(fig01,'currentaxes',hphase);
hold on; pphc = plot(wc,phc,'-g'); set(pphc,'linewidth',1);
hold on; pwgcc = plot([wgcc wgcc],[-180 0],'-.b');
hold on; ppmc = plot([wgcc wgcc],[-180+pmc -180],'-g');
set(ppmc,'linewidth',1);

%%%%% 位相線図のプロット %%%%% L1
set(fig01,'currentaxes',hphase);
hold on; pphl1 = plot(wl1,phl1,'-m'); set(pphl1,'linewidth',1);
hold on; pwgcl1 = plot([wgcl1 wgcl1],[-180 0],'-.b');
hold on; ppml1 = plot([wgcl1 wgcl1],[-180+pml1 -180],'-r');
set(ppml1,'linewidth',1);

%%%%% 位相線図のプロット %%%%% L2
set(fig01,'currentaxes',hphase);
hold on; pphl2 = plot(wl2,phl2,'-b'); set(pphl2,'linewidth',2);
hold on; pwgcl2 = plot([wgcl2 wgcl2],[-180 0],'-.b');
hold on; ppml2 = plot([wgcl2 wgcl2],[-180+pml2 -180],'-r');
set(ppml2,'linewidth',2);

%%%%% コメント
set(fig01,'currentaxes',hcomment);
tgain = text(0.5,0.55,'(a) gain',...
	     'fontname','helvetica',...
	     'fontsize',14);
tgain = text(0.5,0.05,'(b) phase',...
	     'fontname','helvetica',...
	     'fontsize',14);
swgcl2 = {sprintf('\\omega_{gc}=%2.1f',wgcl2)};
text(0.77,0.85,swgcl2,...
     'FontName','Times',...
     'FontSize',14,...
     'Color','b');
swgcl2d = {sprintf('phase-lag compensation')};
text(0.55,0.94,swgcl2d,...
     'FontName','Times',...
     'FontSize',14,...
     'Color','b');
swgcl2dd = {sprintf('k=%2.1f, t=%2.1f',k,t)};
text(0.67,0.9,swgcl2dd,...
     'FontName','Times',...
     'FontSize',14,...
     'Color','b');
%%%%%
tgain = text(0.01,0.01,'',...
	     'fontname','helvetica',...
	     'fontsize',8);

%%%%%%%%%% PI 補償ステップ応答のプロット
fig02 = figure(2); clf;
set(fig02,'position',[600 140 560 750]);

%%%%% PI 補償ステップ応答の axes
houtput = axes('position',[0.2 0.65 0.75 0.34],...
	     'box','on',...
	     'fontName','Helvetica',...
	     'fontsize',14,...
	     'xscale','linear',...
	     'xlim',[0 1.5],...
	     'xtick',[0 0.5 1 1.5],...
	     'xgrid','on',...
	     'ylim',[0 1.4],...
	     'ytick',[0 0.5 1],...
	     'ygrid','on',...
	     'gridlinestyle',':');
set(get(houtput,'xlabel'),...
    'string','\itt \rm[s]',...
    'fontname','helvetica',...
    'fontsize',14); %%%%% x 軸ラベル 
set(get(houtput,'ylabel'),...
    'string','\ity\rm(\itt\rm)',...
    'fontname','helvetica',...
    'fontsize',14); %%%%% y 軸ラベル

%%%%% 入力の axes
hinput = axes('position',[0.2 0.15 0.75 0.34],...
	      'visible','off',...
	      'box','on',...
	      'fontname','Helvetica',...
	      'fontsize',14,...
	      'xscale','log',...
	      'xlim',[1e-2 1e2],...
	      'xtick',[1e-2 1e-1 1e0 1e1 1e2],...
	      'xgrid','off',...
	      'ylim',[-200 0],...
	      'ytick',[-180 -135 -90 -45 0],...
	      'ygrid','on',...
	      'gridlinestyle',':');
set(fig02,'currentaxes',hinput); %%%%% x 軸のグリッドを書いておく
% hold on; plot([1e-0 1e-0],[-200 0],':k',...
% 	      [1e1 1e1],[-200 0],':k');
set(get(hinput,'xlabel'),'string','\omega[rad/s]',...
		  'fontname','helvetica',...
		  'fontsize',14); %%%%% x 軸ラベル
set(get(hinput,'ylabel'),'string','input [deg]',...
		  'fontname','helvetica',...
		  'fontsize',14); %%%%% y 軸ラベル

%%%%% コメント用の axes
hcomment = axes('position',[0 0 1 1],...
		'visible','off',...
		'xlim',[0 1],...
		'ylim',[0 1]);

%%%%% 出力 y のプロット %%%%% P
set(fig02,'currentaxes',houtput);
hold on; pyt = plot(tt,yt,'-c'); set(pyt,'linewidth',1);

%%%%% p 補償ステップ応答のプロット %%%%% L1
set(fig02,'currentaxes',houtput);
hold on; pyt1 = plot(tt,yt1,'-m'); set(pyt1,'linewidth',1);

%%%%% PI 補償ステップ応答のプロット %%%%% L2
set(fig02,'currentaxes',houtput);
hold on; pyt2 = plot(tt2,yt2,'-b'); set(pyt2,'linewidth',2);

%%%%% コメント
set(fig02,'currentaxes',hcomment);
tgain = text(0.5,0.55,'(a) output \ity\rm(\itt\rm)',...
	     'fontname','helvetica',...
	     'fontsize',14);
%%%%%
tgain = text(0.01,0.01,'',...
	     'fontname','helvetica',...
	     'fontsize',8);
%%%%%
swgcl1d = {sprintf('phase-lag compensation')};
text(0.55,0.95,swgcl1d,...
     'FontName','Times',...
     'FontSize',14,...
     'Color','b');
swgcl1d = {sprintf('k=%2.1f, t=%2.1f',k,t)};
text(0.7,0.92,swgcl1d,...
     'FontName','Times',...
     'FontSize',14,...
     'Color','b');

pause % press any key to continue

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 位相遅れ補償によるボード線図とステップ応答が表示されました．
% 水色の線が補償前，ピンク色の線がゲイン補償，青色の線が位相遅れ補償後の
% 応答を表しています．また，緑色の線は，位相遅れ補償器の応答を表しています．
%
% 位相遅れ補償後の位相余裕が十分に確保されているかどうか確認してください．
% また，ステップ応答の過渡特性と定常特性が改善されているかどうかを
% 確認してください．
% 
% 確認できれば，Figure 1 とFigure 2 をそれぞれ印刷してください．
%
% もし，過渡特性および定常特性が改善されていないと判断した場合は，
% 再度設計を行ってください．
% 
% ここで，位相遅れ補償後のゲインと位相は，位相遅れ補償前のゲインと位相に，
% 位相遅れ (PI) 補償器のゲインと位相を足し合わせて得られることに注意
% してください．
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

pause % press any key to continue

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 最後に，位相遅れ補償のパラメータを k = 10, t = 10 に設定した場合と
% k = 10, t = 0.1 に設定した場合の制御系の応答について調べておきます．
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% --------------------------------------------------------------------
% パラメータを k = 10, t = 10 に設定した場合の位相遅れ補償による
% 制御系の応答を表示します．
% --------------------------------------------------------------------

%%%%% パラメータ ｋ, t
k = 10;
t = 10;

pause % press any key to continue

%%%%% 開ループ l1
p = tf([20],[1 11 10]);
c1 = k;
l1 = p*c1;

%%%%% ゲイン補償による閉ループ系は次で与えられます．
t1 = feedback(l1,1);

%%%%% l1 bode 線図のデータ
[magl1,phasel1,wl1]=bode(l1,{1e-2,1e2});
[gml1,pml1,wpcl1,wgcl1] = margin(l1);
logmagl1 = 20*log10(arr2vec(magl1,wl1));
phl1 = arr2vec(phasel1,wl1);

%%%%% 閉ループ t ステップ応答のデータ
[yt1,tt1] = lsim(t1,uu,time);

%%%%%%%%%%%%%%% p 補償 bode 線図のプロット
fig01 = figure(3); clf;
set(fig01,'position',[20 140 560 750]);

%%%%% gain 線図の axes
hgain = axes('position',[0.2 0.65 0.75 0.34],...
	     'box','on',...
	     'fontName','Helvetica',...
	     'fontsize',14,...
	     'xscale','log',...
	     'xlim',[1e-2 1e2],...
	     'xtick',[1e-2 1e-1 1e0 1e1 1e2],...
	     'xgrid','off',...
	     'ylim',[-30 50],...
	     'ytick',[-20 0 20 40],...
	     'ygrid','on',...
	     'gridlinestyle',':');
set(fig01,'currentaxes',hgain); %%%%% x 軸のグリッドを書いておく
hold on; plot([1e-1 1e-1],[100 -100],':k',[1e-0 1e-0],[100 -100],':k',...
	      [1e1 1e1],[100 -100],':k');
set(get(hgain,'xlabel'),...
    'string','\omega[rad/s]',...
    'fontname','helvetica',...
    'fontsize',14); %%%%% x 軸ラベル 
set(get(hgain,'ylabel'),...
    'string','gain [dB]',...
    'fontname','helvetica',...
    'fontsize',14); %%%%% y 軸ラベル
set(fig01,'currentaxes',hgain); %%%%% 0 [dB] の線 
hold on; l0gain = plot([1e-2 1e2],[0 0],'-.b');

%%%%% 位相線図の axes
hphase = axes('position',[0.2 0.15 0.75 0.34],...
	      'box','on',...
	      'fontname','Helvetica',...
	      'fontsize',14,...
	      'xscale','log',...
	      'xlim',[1e-2 1e2],...
	      'xtick',[1e-2 1e-1 1e0 1e1 1e2],...
	      'xgrid','off',...
	      'ylim',[-200 0],...
	      'ytick',[-180 -135 -90 -45 0],...
	      'ygrid','on',...
	      'gridlinestyle',':');
set(fig01,'currentaxes',hphase); %%%%% x 軸のグリッドを書いておく
hold on; plot([1e-1 1e-1],[-200 0],':k',[1e-0 1e-0],[-200 0],':k',...
	      [1e1 1e1],[-200 0],':k');
set(get(hphase,'xlabel'),'string','\omega[rad/s]',...
		  'fontname','helvetica',...
		  'fontsize',14); %%%%% x 軸ラベル
set(get(hphase,'ylabel'),'string','phase [deg]',...
		  'fontname','helvetica',...
		  'fontsize',14); %%%%% y 軸ラベル
set(fig01,'currentaxes',hphase); %%%%% -180 [deg] の線 
hold on; lm180phase = plot([1e-2 1e2],[-180 -180],'-.b');

%%%%% コメント用の axes
hcommentb = axes('position',[0 0 1 1],...
		'visible','off',...
		'xlim',[0 1],...
		'ylim',[0 1]);

%%%%% gain 線図のプロット %%%%% P
set(fig01,'currentaxes',hgain);
hold on; plogmagp = plot(wp,logmagp,'-c'); set(plogmagp,'linewidth',1);
hold on; pwgp = plot([wgcp wgcp],[0 -30],'-.b');

%%%%% gain 線図のプロット %%%%% C
set(fig01,'currentaxes',hgain);
hold on; plot([10^-2 10^2],[20 20],'g');

%%%%% gain 線図のプロット %%%%% L1
set(fig01,'currentaxes',hgain);
hold on; plogmagl1 = plot(wl1,logmagl1,'-m'); set(plogmagl1,'linewidth',2);
hold on; pwgp = plot([wgcl1 wgcl1],[0 -30],'-.b');

%%%%% 位相線図のプロット %%%%% P
set(fig01,'currentaxes',hphase);
hold on; pphp = plot(wp,php,'-c'); set(pphp,'linewidth',1);
hold on; pwgcp = plot([wgcp wgcp],[-180 0],'-.b');
hold on; ppmp = plot([wgcp wgcp],[-180+pmp -180],'-r');
set(ppmp,'linewidth',1);

%%%%% 位相線図のプロット %%%%% L1
set(fig01,'currentaxes',hphase);
hold on; pphl1 = plot(wl1,phl1,'-m'); set(pphl1,'linewidth',2);
hold on; pwgcl1 = plot([wgcl1 wgcl1],[-180 0],'-.b');
hold on; ppml1 = plot([wgcl1 wgcl1],[-180+pml1 -180],'-r');
set(ppml1,'linewidth',2);

%%%%% コメント
set(fig01,'currentaxes',hcommentb);
tgain = text(0.5,0.55,'(a) gain',...
	     'fontname','helvetica',...
	     'fontsize',14);
tgain = text(0.5,0.05,'(b) phase',...
	     'fontname','helvetica',...
	     'fontsize',14);
swgcl1 = {sprintf('\\omega_{gc}=%2.1f',wgcl1)};
text(0.77,0.85,swgcl1,...
     'FontName','Times',...
     'FontSize',14,...
     'Color','b');
swgcl1d = {sprintf('gain compensation k=%2.1f',k)};
text(0.47,0.93,swgcl1d,...
     'FontName','Times',...
     'FontSize',14,...
     'Color','b');

%%%%%%%%%% p 補償ステップ応答のプロット
fig02 = figure(3); clf;
set(fig02,'position',[600 140 560 750]);

%%%%% ステップ応答の axes
houtput = axes('position',[0.2 0.65 0.75 0.34],...
	     'box','on',...
	     'fontName','Helvetica',...
	     'fontsize',14,...
	     'xscale','linear',...
	     'xlim',[0 1.5],...
	     'xtick',[0 0.5 1 1.5],...
	     'xgrid','on',...
	     'ylim',[0 1.4],...
	     'ytick',[0 0.5 1],...
	     'ygrid','on',...
	     'gridlinestyle',':');
set(get(houtput,'xlabel'),...
    'string','\itt \rm[s]',...
    'fontname','helvetica',...
    'fontsize',14); %%%%% x 軸ラベル 
set(get(houtput,'ylabel'),...
    'string','\ity\rm(\itt\rm)',...
    'fontname','helvetica',...
    'fontsize',14); %%%%% y 軸ラベル

%%%%% 入力の axes
%%%%% コメント用の axes
hcomment = axes('position',[0 0 1 1],...
		'visible','off',...
		'xlim',[0 1],...
		'ylim',[0 1]);

%%%%% 出力 y のプロット P
set(fig02,'currentaxes',houtput);
hold on; pyt = plot(tt,yt,'-c'); set(pyt,'linewidth',1);

%%%%% 出力 y のプロット L1
set(fig02,'currentaxes',houtput);
hold on; pyt1 = plot(tt1,yt1,'-m'); set(pyt1,'linewidth',2);

%%%%% コメント
set(fig02,'currentaxes',hcomment);
tgain = text(0.5,0.55,'(a) output \ity\rm(\itt\rm)',...
	     'fontname','helvetica',...
	     'fontsize',14);
%%%%%
swgcl1d = {sprintf('gain compensation k=%2.1f',k)};
text(0.5,0.93,swgcl1d,...
     'FontName','Times',...
     'FontSize',14,...
     'Color','b');

%%%%% コメント用の axes
hcomment = axes('position',[0 0 1 1],...
		'visible','off',...
		'xlim',[0 1],...
		'ylim',[0 1]);

%%%%% 出力 y のプロット %%%%% P
set(fig02,'currentaxes',houtput);
hold on; pyt = plot(tt,yt,'-c'); set(pyt,'linewidth',1);

%%%%% p 補償ステップ応答のプロット %%%%% L1
set(fig02,'currentaxes',houtput);
hold on; pyt1 = plot(tt,yt1,'-m'); set(pyt1,'linewidth',2);

%%%%% パラメータ t
t = 10;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% これより，位相遅れ (PI)補償器，位相遅れ補償による開ループ伝達関数，
% および閉ループ伝達関数は次式で与えられます．
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%% 位相遅れ (PI) 補償器は，次式で与えられます．
c2 = tf([k*t k],[t 0]);
zpk(c2)

%%% 位相遅れ補償による開ループ伝達関数は，次式で与えられます．
l2 = p*c2;
zpk(l2)

%%%%% 位相遅れ補償による閉ループ伝達関数は，次式で与えられます．
t2 = feedback(l2,1);
zpk(t2)

%%%%% controller bode 線図のデータ
[magc,phasec,wc]=bode(c2,{1e-2,1e2});
[gmc,pmc,wpcc,wgcc] = margin(c2);
logmagc = 20*log10(arr2vec(magc,wc));
phc = arr2vec(phasec,wc);

%%%%% l2 bode 線図のデータ
[magl2,phasel2,wl2] = bode(l2,{1e-2,1e2});
[gml2,pml2,wpcl2,wgcl2] = margin(l2);
logmagl2 = 20*log10(arr2vec(magl2,wl2));
phl2 = arr2vec(phasel2,wl2);

%%%%% 閉ループ t のステップ応答のデータ
[yt2,tt2] = lsim(t2,uu,time);

%%%%%%%%%%%%%%% pi 補償 bode 線図のプロット
fig01 = figure(3); clf;
set(fig01,'position',[20 140 560 750]);

%%%%% gain 線図の axes
hgain = axes('position',[0.2 0.65 0.75 0.34],...
	     'box','on',...
	     'fontName','Helvetica',...
	     'fontsize',14,...
	     'xscale','log',...
	     'xlim',[1e-2 1e2],...
	     'xtick',[1e-2 1e-1 1e0 1e1 1e2],...
	     'xgrid','off',...
	     'ylim',[-30 50],...
	     'ytick',[-20 0 20 40],...
	     'ygrid','on',...
	     'gridlinestyle',':');
set(fig01,'currentaxes',hgain); %%%%% x 軸のグリッドを書いておく
hold on; plot([1e-1 1e-1],[100 -100],':k',[1e-0 1e-0],[100 -100],':k',...
	      [1e1 1e1],[100 -100],':k');
set(get(hgain,'xlabel'),...
    'string','\omega[rad/s]',...
    'fontname','helvetica',...
    'fontsize',14); %%%%% x 軸ラベル 
set(get(hgain,'ylabel'),...
    'string','gain [dB]',...
    'fontname','helvetica',...
    'fontsize',14); %%%%% y 軸ラベル
set(fig01,'currentaxes',hgain); %%%%% 0 [dB] の線 
hold on; l0gain = plot([1e-2 1e2],[0 0],'-.b');

%%%%% 位相線図の axes
hphase = axes('position',[0.2 0.15 0.75 0.34],...
	      'box','on',...
	      'fontname','Helvetica',...
	      'fontsize',14,...
	      'xscale','log',...
	      'xlim',[1e-2 1e2],...
	      'xtick',[1e-2 1e-1 1e0 1e1 1e2],...
	      'xgrid','off',...
	      'ylim',[-200 0],...
	      'ytick',[-180 -135 -90 -45 0],...
	      'ygrid','on',...
	      'gridlinestyle',':');
set(fig01,'currentaxes',hphase); %%%%% x 軸のグリッドを書いておく
hold on; plot([1e-1 1e-1],[-200 0],':k',[1e-0 1e-0],[-200 0],':k',...
	      [1e1 1e1],[-200 0],':k');
set(get(hphase,'xlabel'),'string','\omega[rad/s]',...
		  'fontname','helvetica',...
		  'fontsize',14); %%%%% x 軸ラベル
set(get(hphase,'ylabel'),'string','phase [deg]',...
		  'fontname','helvetica',...
		  'fontsize',14); %%%%% y 軸ラベル
set(fig01,'currentaxes',hphase); %%%%% -180 [deg] の線 
hold on; lm180phase = plot([1e-2 1e2],[-180 -180],'-.b');

%%%%% コメント用の axes
hcomment = axes('position',[0 0 1 1],...
		'visible','off',...
		'xlim',[0 1],...
		'ylim',[0 1]);

%%%%% gain 線図のプロット %%%%% P
set(fig01,'currentaxes',hgain);
hold on; plogmagp = plot(wp,logmagp,'-c'); set(plogmagp,'linewidth',1);
hold on; pwgp = plot([wgcp wgcp],[0 -30],'-.b');

%%%%% gain 線図のプロット %%%%% C
set(fig01,'currentaxes',hgain);
hold on; plogmagc = plot(wc,logmagc,'-g'); set(plogmagc,'linewidth',1);
hold on; pwgc = plot([wgcc wgcc],[0 -30],'-.g');

%%%%% gain 線図のプロット %%%%% L1
set(fig01,'currentaxes',hgain);
hold on; plogmagl1 = plot(wl1,logmagl1,'-m'); set(plogmagl1,'linewidth',1);
hold on; pwgp = plot([wgcl1 wgcl1],[0 -30],'-.b');

%%%%% gain 線図のプロット %%%%% L2
set(fig01,'currentaxes',hgain);
hold on; plogmagl2 = plot(wl2,logmagl2,'-b'); set(plogmagl2,'linewidth',2);
hold on; pwgp = plot([wgcl2 wgcl2],[0 -30],'-.b');

%%%%% 位相線図のプロット %%%%% P
set(fig01,'currentaxes',hphase);
hold on; pphp = plot(wp,php,'-c'); set(pphp,'linewidth',1);
hold on; pwgcp = plot([wgcp wgcp],[-180 0],'-.b');
hold on; ppmp = plot([wgcp wgcp],[-180+pmp -180],'-r');
set(ppmp,'linewidth',1);

%%%%% 位相線図のプロット %%%%% C
set(fig01,'currentaxes',hphase);
hold on; pphc = plot(wc,phc,'-g'); set(pphc,'linewidth',1);
hold on; pwgcc = plot([wgcc wgcc],[-180 0],'-.b');
hold on; ppmc = plot([wgcc wgcc],[-180+pmc -180],'-g');
set(ppmc,'linewidth',1);

%%%%% 位相線図のプロット %%%%% L1
set(fig01,'currentaxes',hphase);
hold on; pphl1 = plot(wl1,phl1,'-m'); set(pphl1,'linewidth',1);
hold on; pwgcl1 = plot([wgcl1 wgcl1],[-180 0],'-.b');
hold on; ppml1 = plot([wgcl1 wgcl1],[-180+pml1 -180],'-r');
set(ppml1,'linewidth',1);

%%%%% 位相線図のプロット %%%%% L2
set(fig01,'currentaxes',hphase);
hold on; pphl2 = plot(wl2,phl2,'-b'); set(pphl2,'linewidth',2);
hold on; pwgcl2 = plot([wgcl2 wgcl2],[-180 0],'-.b');
hold on; ppml2 = plot([wgcl2 wgcl2],[-180+pml2 -180],'-r');
set(ppml2,'linewidth',2);

%%%%% コメント
set(fig01,'currentaxes',hcomment);
tgain = text(0.5,0.55,'(a) gain',...
	     'fontname','helvetica',...
	     'fontsize',14);
tgain = text(0.5,0.05,'(b) phase',...
	     'fontname','helvetica',...
	     'fontsize',14);
swgcl2 = {sprintf('\\omega_{gc}=%2.1f',wgcl2)};
text(0.77,0.85,swgcl2,...
     'FontName','Times',...
     'FontSize',14,...
     'Color','b');
swgcl2d = {sprintf('phase-lag compensation')};
text(0.55,0.94,swgcl2d,...
     'FontName','Times',...
     'FontSize',14,...
     'Color','b');
swgcl2dd = {sprintf('k=%2.1f, t=%2.1f',k,t)};
text(0.67,0.9,swgcl2dd,...
     'FontName','Times',...
     'FontSize',14,...
     'Color','b');
%%%%%
tgain = text(0.01,0.01,'',...
	     'fontname','helvetica',...
	     'fontsize',8);

%%%%%%%%%% PI 補償ステップ応答のプロット
fig02 = figure(4); clf;
set(fig02,'position',[600 140 560 750]);

%%%%% PI 補償ステップ応答の axes
houtput = axes('position',[0.2 0.65 0.75 0.34],...
	     'box','on',...
	     'fontName','Helvetica',...
	     'fontsize',14,...
	     'xscale','linear',...
	     'xlim',[0 1.5],...
	     'xtick',[0 0.5 1 1.5],...
	     'xgrid','on',...
	     'ylim',[0 1.4],...
	     'ytick',[0 0.5 1],...
	     'ygrid','on',...
	     'gridlinestyle',':');
set(get(houtput,'xlabel'),...
    'string','\itt \rm[s]',...
    'fontname','helvetica',...
    'fontsize',14); %%%%% x 軸ラベル 
set(get(houtput,'ylabel'),...
    'string','\ity\rm(\itt\rm)',...
    'fontname','helvetica',...
    'fontsize',14); %%%%% y 軸ラベル

%%%%% 入力の axes
hinput = axes('position',[0.2 0.15 0.75 0.34],...
	      'visible','off',...
	      'box','on',...
	      'fontname','Helvetica',...
	      'fontsize',14,...
	      'xscale','log',...
	      'xlim',[1e-2 1e2],...
	      'xtick',[1e-2 1e-1 1e0 1e1 1e2],...
	      'xgrid','off',...
	      'ylim',[-200 0],...
	      'ytick',[-180 -135 -90 -45 0],...
	      'ygrid','on',...
	      'gridlinestyle',':');
set(fig02,'currentaxes',hinput); %%%%% x 軸のグリッドを書いておく
set(get(hinput,'xlabel'),'string','\omega[rad/s]',...
		  'fontname','helvetica',...
		  'fontsize',14); %%%%% x 軸ラベル
set(get(hinput,'ylabel'),'string','input [deg]',...
		  'fontname','helvetica',...
		  'fontsize',14); %%%%% y 軸ラベル

%%%%% コメント用の axes
hcomment = axes('position',[0 0 1 1],...
		'visible','off',...
		'xlim',[0 1],...
		'ylim',[0 1]);

%%%%% 出力 y のプロット %%%%% P
set(fig02,'currentaxes',houtput);
hold on; pyt = plot(tt,yt,'-c'); set(pyt,'linewidth',1);

%%%%% p 補償ステップ応答のプロット %%%%% L1
set(fig02,'currentaxes',houtput);
hold on; pyt1 = plot(tt,yt1,'-m'); set(pyt1,'linewidth',1);

%%%%% PI 補償ステップ応答のプロット %%%%% L2
set(fig02,'currentaxes',houtput);
hold on; pyt2 = plot(tt2,yt2,'-b'); set(pyt2,'linewidth',2);

%%%%% コメント
set(fig02,'currentaxes',hcomment);
tgain = text(0.5,0.55,'(a) output \ity\rm(\itt\rm)',...
	     'fontname','helvetica',...
	     'fontsize',14);
%%%%%
tgain = text(0.01,0.01,'',...
	     'fontname','helvetica',...
	     'fontsize',8);
%%%%%
swgcl1d = {sprintf('phase-lag compensation')};
text(0.55,0.95,swgcl1d,...
     'FontName','Times',...
     'FontSize',14,...
     'Color','b');
swgcl1d = {sprintf('k=%2.1f, t=%2.1f',k,t)};
text(0.7,0.92,swgcl1d,...
     'FontName','Times',...
     'FontSize',14,...
     'Color','b');

pause % press any key to continue
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 位相遅れ補償 (k = 10, t = 10) によるボード線図とステップ応答が
% 表示されました．
% 応答特性を評価してください (考察課題 2)．
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% --------------------------------------------------------------------
% パラメータを k = 10, t = 0.1 に設定した場合の位相遅れ補償による
% 制御系の応答を表示します．
% --------------------------------------------------------------------

%%%%% パラメータ ｋ, t
k = 10;
t = 0.1;

pause; % press any key to continue.

%%%%% 開ループ l1
p = tf([20],[1 11 10]);
c1 = k;
l1 = p*c1;

%%%%% ゲイン補償による閉ループ系は，次で与えられます．
t1 = feedback(l1,1);

%%%%% l1 bode 線図のデータ
[magl1,phasel1,wl1]=bode(l1,{1e-2,1e2});
[gml1,pml1,wpcl1,wgcl1] = margin(l1);
logmagl1 = 20*log10(arr2vec(magl1,wl1));
phl1 = arr2vec(phasel1,wl1);

%%%%% 閉ループ t ステップ応答のデータ
[yt1,tt1] = lsim(t1,uu,time);

%%%%%%%%%%%%%%% p 補償 bode 線図のプロット
fig01 = figure(5); clf;
set(fig01,'position',[20 140 560 750]);

%%%%% gain 線図の axes
hgain = axes('position',[0.2 0.65 0.75 0.34],...
	     'box','on',...
	     'fontName','Helvetica',...
	     'fontsize',14,...
	     'xscale','log',...
	     'xlim',[1e-2 1e2],...
	     'xtick',[1e-2 1e-1 1e0 1e1 1e2],...
	     'xgrid','off',...
	     'ylim',[-30 50],...
	     'ytick',[-20 0 20 40],...
	     'ygrid','on',...
	     'gridlinestyle',':');
set(fig01,'currentaxes',hgain); %%%%% x 軸のグリッドを書いておく
hold on; plot([1e-1 1e-1],[100 -100],':k',[1e-0 1e-0],[100 -100],':k',...
	      [1e1 1e1],[100 -100],':k');
set(get(hgain,'xlabel'),...
    'string','\omega[rad/s]',...
    'fontname','helvetica',...
    'fontsize',14); %%%%% x 軸ラベル 
set(get(hgain,'ylabel'),...
    'string','gain [dB]',...
    'fontname','helvetica',...
    'fontsize',14); %%%%% y 軸ラベル
set(fig01,'currentaxes',hgain); %%%%% 0 [dB] の線 
hold on; l0gain = plot([1e-2 1e2],[0 0],'-.b');

%%%%% 位相線図の axes
hphase = axes('position',[0.2 0.15 0.75 0.34],...
	      'box','on',...
	      'fontname','Helvetica',...
	      'fontsize',14,...
	      'xscale','log',...
	      'xlim',[1e-2 1e2],...
	      'xtick',[1e-2 1e-1 1e0 1e1 1e2],...
	      'xgrid','off',...
	      'ylim',[-200 0],...
	      'ytick',[-180 -135 -90 -45 0],...
	      'ygrid','on',...
	      'gridlinestyle',':');
set(fig01,'currentaxes',hphase); %%%%% x 軸のグリッドを書いておく
hold on; plot([1e-1 1e-1],[-200 0],':k',[1e-0 1e-0],[-200 0],':k',...
	      [1e1 1e1],[-200 0],':k');
set(get(hphase,'xlabel'),'string','\omega[rad/s]',...
		  'fontname','helvetica',...
		  'fontsize',14); %%%%% x 軸ラベル
set(get(hphase,'ylabel'),'string','phase [deg]',...
		  'fontname','helvetica',...
		  'fontsize',14); %%%%% y 軸ラベル
set(fig01,'currentaxes',hphase); %%%%% -180 [deg] の線 
hold on; lm180phase = plot([1e-2 1e2],[-180 -180],'-.b');

%%%%% コメント用の axes
hcommentb = axes('position',[0 0 1 1],...
		'visible','off',...
		'xlim',[0 1],...
		'ylim',[0 1]);

%%%%% gain 線図のプロット %%%%% P
set(fig01,'currentaxes',hgain);
hold on; plogmagp = plot(wp,logmagp,'-c'); set(plogmagp,'linewidth',1);
hold on; pwgp = plot([wgcp wgcp],[0 -30],'-.b');

%%%%% gain 線図のプロット %%%%% C
set(fig01,'currentaxes',hgain);
hold on; plot([10^-2 10^2],[20 20],'g');

%%%%% gain 線図のプロット %%%%% L1
set(fig01,'currentaxes',hgain);
hold on; plogmagl1 = plot(wl1,logmagl1,'-m'); set(plogmagl1,'linewidth',2);
hold on; pwgp = plot([wgcl1 wgcl1],[0 -30],'-.b');

%%%%% 位相線図のプロット %%%%% P
set(fig01,'currentaxes',hphase);
hold on; pphp = plot(wp,php,'-c'); set(pphp,'linewidth',1);
hold on; pwgcp = plot([wgcp wgcp],[-180 0],'-.b');
hold on; ppmp = plot([wgcp wgcp],[-180+pmp -180],'-r');
set(ppmp,'linewidth',1);

%%%%% 位相線図のプロット %%%%% L1
set(fig01,'currentaxes',hphase);
hold on; pphl1 = plot(wl1,phl1,'-m'); set(pphl1,'linewidth',2);
hold on; pwgcl1 = plot([wgcl1 wgcl1],[-180 0],'-.b');
hold on; ppml1 = plot([wgcl1 wgcl1],[-180+pml1 -180],'-r');
set(ppml1,'linewidth',2);

%%%%% コメント
set(fig01,'currentaxes',hcommentb);
tgain = text(0.5,0.55,'(a) gain',...
	     'fontname','helvetica',...
	     'fontsize',14);
tgain = text(0.5,0.05,'(b) phase',...
	     'fontname','helvetica',...
	     'fontsize',14);
swgcl1 = {sprintf('\\omega_{gc}=%2.1f',wgcl1)};
text(0.77,0.85,swgcl1,...
     'FontName','Times',...
     'FontSize',14,...
     'Color','b');
swgcl1d = {sprintf('gain compensation k=%2.1f',k)};
text(0.47,0.93,swgcl1d,...
     'FontName','Times',...
     'FontSize',14,...
     'Color','b');

%%%%%%%%%% p 補償ステップ応答のプロット
fig02 = figure(6); clf;
set(fig02,'position',[600 140 560 750]);

%%%%% ステップ応答の axes
houtput = axes('position',[0.2 0.65 0.75 0.34],...
	     'box','on',...
	     'fontName','Helvetica',...
	     'fontsize',14,...
	     'xscale','linear',...
	     'xlim',[0 1.5],...
	     'xtick',[0 0.5 1 1.5],...
	     'xgrid','on',...
	     'ylim',[0 1.4],...
	     'ytick',[0 0.5 1],...
	     'ygrid','on',...
	     'gridlinestyle',':');
set(get(houtput,'xlabel'),...
    'string','\itt \rm[s]',...
    'fontname','helvetica',...
    'fontsize',14); %%%%% x 軸ラベル 
set(get(houtput,'ylabel'),...
    'string','\ity\rm(\itt\rm)',...
    'fontname','helvetica',...
    'fontsize',14); %%%%% y 軸ラベル

%%%%% 入力の axes
%%%%% コメント用の axes
hcomment = axes('position',[0 0 1 1],...
		'visible','off',...
		'xlim',[0 1],...
		'ylim',[0 1]);

%%%%% 出力 y のプロット P
set(fig02,'currentaxes',houtput);
hold on; pyt = plot(tt,yt,'-c'); set(pyt,'linewidth',1);

%%%%% 出力 y のプロット L1
set(fig02,'currentaxes',houtput);
hold on; pyt1 = plot(tt1,yt1,'-m'); set(pyt1,'linewidth',2);

%%%%% コメント
set(fig02,'currentaxes',hcomment);
tgain = text(0.5,0.55,'(a) output \ity\rm(\itt\rm)',...
	     'fontname','helvetica',...
	     'fontsize',14);
%%%%%
swgcl1d = {sprintf('gain compensation k=%2.1f',k)};
text(0.5,0.93,swgcl1d,...
     'FontName','Times',...
     'FontSize',14,...
     'Color','b');

%%%%% コメント用の axes
hcomment = axes('position',[0 0 1 1],...
		'visible','off',...
		'xlim',[0 1],...
		'ylim',[0 1]);

%%%%% 出力 y のプロット %%%%% P
set(fig02,'currentaxes',houtput);
hold on; pyt = plot(tt,yt,'-c'); set(pyt,'linewidth',1);

%%%%% p 補償ステップ応答のプロット %%%%% L1
set(fig02,'currentaxes',houtput);
hold on; pyt1 = plot(tt,yt1,'-m'); set(pyt1,'linewidth',2);

%%%%% パラメータ t
t = 0.1;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% これより，位相遅れ (PI) 補償器，位相遅れ補償による開ループ伝達関数，
% および閉ループ伝達関数は次式で与えられます．
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%% 位相遅れ (PI) 補償器は，次式で与えられます．
c2 = tf([k*t k],[t 0]);
zpk(c2)

%%% 位相遅れ補償による開ループ伝達関数は，次式で与えられます．
l2 = p*c2;
zpk(l2)

%%%%% 位相遅れ補償による閉ループ伝達関数は，次式で与えられます．
t2 = feedback(l2,1);
zpk(t2)

%%%%% controller bode 線図のデータ
[magc,phasec,wc]=bode(c2,{1e-2,1e2});
[gmc,pmc,wpcc,wgcc] = margin(c2);
logmagc = 20*log10(arr2vec(magc,wc));
phc = arr2vec(phasec,wc);

%%%%% l2 bode 線図のデータ
[magl2,phasel2,wl2] = bode(l2,{1e-2,1e2});
[gml2,pml2,wpcl2,wgcl2] = margin(l2);
logmagl2 = 20*log10(arr2vec(magl2,wl2));
phl2 = arr2vec(phasel2,wl2);

%%%%% 閉ループ t のステップ応答のデータ
[yt2,tt2] = lsim(t2,uu,time);

%%%%%%%%%%%%%%% pi 補償 bode 線図のプロット
fig01 = figure(5); clf;
set(fig01,'position',[20 140 560 750]);

%%%%% gain 線図の axes
hgain = axes('position',[0.2 0.65 0.75 0.34],...
	     'box','on',...
	     'fontName','Helvetica',...
	     'fontsize',14,...
	     'xscale','log',...
	     'xlim',[1e-2 1e2],...
	     'xtick',[1e-2 1e-1 1e0 1e1 1e2],...
	     'xgrid','off',...
	     'ylim',[-30 50],...
	     'ytick',[-20 0 20 40],...
	     'ygrid','on',...
	     'gridlinestyle',':');
set(fig01,'currentaxes',hgain); %%%%% x 軸のグリッドを書いておく
hold on; plot([1e-1 1e-1],[100 -100],':k',[1e-0 1e-0],[100 -100],':k',...
	      [1e1 1e1],[100 -100],':k');
set(get(hgain,'xlabel'),...
    'string','\omega[rad/s]',...
    'fontname','helvetica',...
    'fontsize',14); %%%%% x 軸ラベル 
set(get(hgain,'ylabel'),...
    'string','gain [dB]',...
    'fontname','helvetica',...
    'fontsize',14); %%%%% y 軸ラベル
set(fig01,'currentaxes',hgain); %%%%% 0 [dB] の線 
hold on; l0gain = plot([1e-2 1e2],[0 0],'-.b');

%%%%% 位相線図の axes
hphase = axes('position',[0.2 0.15 0.75 0.34],...
	      'box','on',...
	      'fontname','Helvetica',...
	      'fontsize',14,...
	      'xscale','log',...
	      'xlim',[1e-2 1e2],...
	      'xtick',[1e-2 1e-1 1e0 1e1 1e2],...
	      'xgrid','off',...
	      'ylim',[-200 0],...
	      'ytick',[-180 -135 -90 -45 0],...
	      'ygrid','on',...
	      'gridlinestyle',':');
set(fig01,'currentaxes',hphase); %%%%% x 軸のグリッドを書いておく
hold on; plot([1e-1 1e-1],[-200 0],':k',[1e-0 1e-0],[-200 0],':k',...
	      [1e1 1e1],[-200 0],':k');
set(get(hphase,'xlabel'),'string','\omega[rad/s]',...
		  'fontname','helvetica',...
		  'fontsize',14); %%%%% x 軸ラベル
set(get(hphase,'ylabel'),'string','phase [deg]',...
		  'fontname','helvetica',...
		  'fontsize',14); %%%%% y 軸ラベル
set(fig01,'currentaxes',hphase); %%%%% -180 [deg] の線 
hold on; lm180phase = plot([1e-2 1e2],[-180 -180],'-.b');

%%%%% コメント用の axes
hcomment = axes('position',[0 0 1 1],...
		'visible','off',...
		'xlim',[0 1],...
		'ylim',[0 1]);

%%%%% gain 線図のプロット %%%%% P
set(fig01,'currentaxes',hgain);
hold on; plogmagp = plot(wp,logmagp,'-c'); set(plogmagp,'linewidth',1);
hold on; pwgp = plot([wgcp wgcp],[0 -30],'-.b');

%%%%% gain 線図のプロット %%%%% C
set(fig01,'currentaxes',hgain);
hold on; plogmagc = plot(wc,logmagc,'-g'); set(plogmagc,'linewidth',1);
hold on; pwgc = plot([wgcc wgcc],[0 -30],'-.g');

%%%%% gain 線図のプロット %%%%% L1
set(fig01,'currentaxes',hgain);
hold on; plogmagl1 = plot(wl1,logmagl1,'-m'); set(plogmagl1,'linewidth',1);
hold on; pwgp = plot([wgcl1 wgcl1],[0 -30],'-.b');

%%%%% gain 線図のプロット %%%%% L2
set(fig01,'currentaxes',hgain);
hold on; plogmagl2 = plot(wl2,logmagl2,'-b'); set(plogmagl2,'linewidth',2);
hold on; pwgp = plot([wgcl2 wgcl2],[0 -30],'-.b');

%%%%% 位相線図のプロット %%%%% P
set(fig01,'currentaxes',hphase);
hold on; pphp = plot(wp,php,'-c'); set(pphp,'linewidth',1);
hold on; pwgcp = plot([wgcp wgcp],[-180 0],'-.b');
hold on; ppmp = plot([wgcp wgcp],[-180+pmp -180],'-r');
set(ppmp,'linewidth',1);

%%%%% 位相線図のプロット %%%%% C
set(fig01,'currentaxes',hphase);
hold on; pphc = plot(wc,phc,'-g'); set(pphc,'linewidth',1);
hold on; pwgcc = plot([wgcc wgcc],[-180 0],'-.b');
hold on; ppmc = plot([wgcc wgcc],[-180+pmc -180],'-g');
set(ppmc,'linewidth',1);

%%%%% 位相線図のプロット %%%%% L1
set(fig01,'currentaxes',hphase);
hold on; pphl1 = plot(wl1,phl1,'-m'); set(pphl1,'linewidth',1);
hold on; pwgcl1 = plot([wgcl1 wgcl1],[-180 0],'-.b');
hold on; ppml1 = plot([wgcl1 wgcl1],[-180+pml1 -180],'-r');
set(ppml1,'linewidth',1);

%%%%% 位相線図のプロット %%%%% L2
set(fig01,'currentaxes',hphase);
hold on; pphl2 = plot(wl2,phl2,'-b'); set(pphl2,'linewidth',2);
hold on; pwgcl2 = plot([wgcl2 wgcl2],[-180 0],'-.b');
hold on; ppml2 = plot([wgcl2 wgcl2],[-180+pml2 -180],'-r');
set(ppml2,'linewidth',2);

%%%%% コメント
set(fig01,'currentaxes',hcomment);
tgain = text(0.5,0.55,'(a) gain',...
	     'fontname','helvetica',...
	     'fontsize',14);
tgain = text(0.5,0.05,'(b) phase',...
	     'fontname','helvetica',...
	     'fontsize',14);
swgcl2 = {sprintf('\\omega_{gc}=%2.1f',wgcl2)};
text(0.77,0.85,swgcl2,...
     'FontName','Times',...
     'FontSize',14,...
     'Color','b');
swgcl2d = {sprintf('phase-lag compensation')};
text(0.55,0.94,swgcl2d,...
     'FontName','Times',...
     'FontSize',14,...
     'Color','b');
swgcl2dd = {sprintf('k=%2.1f, t=%2.1f',k,t)};
text(0.67,0.9,swgcl2dd,...
     'FontName','Times',...
     'FontSize',14,...
     'Color','b');
%%%%%
tgain = text(0.01,0.01,'',...
	     'fontname','helvetica',...
	     'fontsize',8);

%%%%%%%%%% PI 補償ステップ応答のプロット
fig02 = figure(6); clf;
set(fig02,'position',[600 140 560 750]);

%%%%% PI 補償ステップ応答の axes
houtput = axes('position',[0.2 0.65 0.75 0.34],...
	     'box','on',...
	     'fontName','Helvetica',...
	     'fontsize',14,...
	     'xscale','linear',...
	     'xlim',[0 1.5],...
	     'xtick',[0 0.5 1 1.5],...
	     'xgrid','on',...
	     'ylim',[0 1.4],...
	     'ytick',[0 0.5 1],...
	     'ygrid','on',...
	     'gridlinestyle',':');
set(get(houtput,'xlabel'),...
    'string','\itt \rm[s]',...
    'fontname','helvetica',...
    'fontsize',14); %%%%% x 軸ラベル 
set(get(houtput,'ylabel'),...
    'string','\ity\rm(\itt\rm)',...
    'fontname','helvetica',...
    'fontsize',14); %%%%% y 軸ラベル

%%%%% 入力の axes
hinput = axes('position',[0.2 0.15 0.75 0.34],...
	      'visible','off',...
	      'box','on',...
	      'fontname','Helvetica',...
	      'fontsize',14,...
	      'xscale','log',...
	      'xlim',[1e-2 1e2],...
	      'xtick',[1e-2 1e-1 1e0 1e1 1e2],...
	      'xgrid','off',...
	      'ylim',[-200 0],...
	      'ytick',[-180 -135 -90 -45 0],...
	      'ygrid','on',...
	      'gridlinestyle',':');
set(fig02,'currentaxes',hinput); %%%%% x 軸のグリッドを書いておく
set(get(hinput,'xlabel'),'string','\omega[rad/s]',...
		  'fontname','helvetica',...
		  'fontsize',14); %%%%% x 軸ラベル
set(get(hinput,'ylabel'),'string','input [deg]',...
		  'fontname','helvetica',...
		  'fontsize',14); %%%%% y 軸ラベル

%%%%% コメント用の axes
hcomment = axes('position',[0 0 1 1],...
		'visible','off',...
		'xlim',[0 1],...
		'ylim',[0 1]);

%%%%% 出力 y のプロット %%%%% P
set(fig02,'currentaxes',houtput);
hold on; pyt = plot(tt,yt,'-c'); set(pyt,'linewidth',1);

%%%%% p 補償ステップ応答のプロット %%%%% L1
set(fig02,'currentaxes',houtput);
hold on; pyt1 = plot(tt,yt1,'-m'); set(pyt1,'linewidth',1);

%%%%% PI 補償ステップ応答のプロット %%%%% L2
set(fig02,'currentaxes',houtput);
hold on; pyt2 = plot(tt2,yt2,'-b'); set(pyt2,'linewidth',2);

%%%%% コメント
set(fig02,'currentaxes',hcomment);
tgain = text(0.5,0.55,'(a) output \ity\rm(\itt\rm)',...
	     'fontname','helvetica',...
	     'fontsize',14);
%%%%%
tgain = text(0.01,0.01,'',...
	     'fontname','helvetica',...
	     'fontsize',8);
%%%%%
swgcl1d = {sprintf('phase-lag compensation')};
text(0.55,0.95,swgcl1d,...
     'FontName','Times',...
     'FontSize',14,...
     'Color','b');
swgcl1d = {sprintf('k=%2.1f, t=%2.1f',k,t)};
text(0.7,0.92,swgcl1d,...
     'FontName','Times',...
     'FontSize',14,...
     'Color','b')
 
pause % press any key to continue.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 位相遅れ補償 (k = 10, t = 0.1) によるボード線図とステップ応答が
% 表示されました．
% 
% 応答特性を評価してください (考察課題 2)．
%
% 以上で，Computer Exercise 3 は終了です．
% レポートの提出準備を進めてください．
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

pause % press any key to continue.
close all;
