clear
format short e
DK_DEF_NAME='dk_defin';
w=logspace(-2,6,100);
%設一般被控對象

%定義控制對象和加權函數
Psys=nd2sys([1],[1  0  0],3.87e7);
W1sys=nd2sys([1  125.7],[1  1e-4],8e-1);
W2num1=[1  1e+4  5.7e+7];
W2dencof=1;
W2den1=[1  1.2e+4  4.04e+8];
W2sys1=nd2sys(W2num1,W2den1);
W2sys=mmult(W2sys1,W2sys1,23.9);
W3sys=0.1;
W4sys=nd2sys([15  15*0.5*0.5e4],[1  10*5e4]);
%顯示加權函數的Bode圖
Pfr=frsp(Psys,w);
W1fr=frsp(W1sys,w);
W2fr=frsp(W2sys,w);
W3fr=frsp(W3sys,w);
W4fr=frsp(W4sys,w);
figure;
vplot('liv,lm',W1fr,W2fr,W3fr,W4fr);
title('Weightign Functions')
xlabel('Freprency [rad/s]')
ylabel('Gain')
grid;
%設一般被控對象
systemnames='Psys W1sys W2sys W3sys W4sys';
inputvar='[w1;w2;u]';
outputvar='[W1sys;W2sys;W4sys;w2+Psys]';
input_to_W3sys='[w1]';
input_to_Psys='[u+W3sys]'
input_to_W1sys='[Psys+w2]';
input_to_W2sys='[Psys]';
input_to_W4sys='[u]';
sysoutname='Gpsys';
cleanupsysic='yes';
sysic;
%μ設計

dkit
K=k_dk2suffix;
 %顯示控制器的Bode圖
Kfr=frsp(K,w)';
figure;
subplot(2,1,1)
vplot('liv,lm',Kfr');
title('Bode plot of Controller')
xlabel('Frepquency[rad/s]')
ylabel('Gain')
grid;
subplot(2,1,2)
vplot('liv,p',Kfr');
xlabel('Frequency[rad/s]')
ylabel('Phase(degrees)')
grid;
%仿真
%使用第一次迭代控制器時
K=k_dk1suffix;

%仿真
systemnames='Psys K';
inputvar='[w;r]';
outputvar='[Psys+r;K]';
input_to_Psys='[K+w]';
input_to_K='[Psys+r]';
sysoutname='Clsys';
cleanupsysic='yes';
sysic;

%仿真（mu控制器）
 
[pa,pb,pc,pd]=unpck(Psys);
[ka,kb,kc,kd]=unpck(K);
[da,db,dc,dd]=unpck(sel(Clsys,1,1));
[ua,ub,uc,ud]=unpck(sel(Clsys,2,1));
%干擾響應
t=[0:0.0001:0.1];
[y,x,t]=step(da,db,dc,dd,1,t);
[u,x,t]=step(ua,ub,uc,ud,1,t);
figure
subplot(2,1,1)
plot(t,y)
axis([0  0.1  0  16])
xlabel('Time[s]')
ylabel('Amplitude')
title('Step disturbance response')
grid on;
%控制輸入
subplot(2,1,2)
plot(t,u)
axis([0  0.01  -1.7  0])
xlabel('Time[s]')
ylabel('u[V]')
title('Input')
grid on


%使用第二次迭代控制器時
K=k_dk2suffix;

%仿真
systemnames='Psys K';
inputvar='[w;r]';
outputvar='[Psys+r;K]';
input_to_Psys='[K+w]';
input_to_K='[Psys+r]';
sysoutname='Clsys';
cleanupsysic='yes';
sysic;

%仿真（mu控制器）
 
[pa,pb,pc,pd]=unpck(Psys);
[ka,kb,kc,kd]=unpck(K);
[da,db,dc,dd]=unpck(sel(Clsys,1,1));
[ua,ub,uc,ud]=unpck(sel(Clsys,2,1));
%干擾響應
t=[0:0.0001:0.1];
[y,x,t]=step(da,db,dc,dd,1,t);
[u,x,t]=step(ua,ub,uc,ud,1,t);
figure
subplot(2,1,1)
plot(t,y)
axis([0  0.1  0  16])
xlabel('Time[s]')
ylabel('Amplitude')
title('Step disturbance response')
grid on;
%控制輸入
subplot(2,1,2)
plot(t,u)
axis([0  0.01  -1.7  0])
xlabel('Time[s]')
ylabel('u[V]')
title('Input')
grid on