% spc_sample.m

clear all
close all

% システム
rotatepend;
n = length(A);
m = size(B,2);
sys = ss(A,B,C,D);

% LQ 制御
Qt = expm(A*h)'*Q*expm(A*h);
Bt = expm(-A*h)*B;
F = -lqr(sys,Q,R);
Ft = F*expm(A*h); 

% 初期時刻
T0 = 0;

% 想定するハードウェアのサンプリング間隔
del = 0.005;

% 台形近似の分割数 (どちらかを選択)
 % サンプリング間隔に依存した分割数
 N = h/del;
 % 任意の分割数
 % N = 4;

% 時間応答の計算
time = 5; % 時間
ts = del/10;
x0 = X0;
t0 = T0;
p_mem = []; c_mem = [];
T = []; X = []; Ud = [];
for kk = 1:1:(time/del)
 % 制御器 (どちらかを選択)
  % プロパ近似
  [input,c_mem] = dd_proper(A,B,Ft,h,N,x0,c_mem);
  % 厳密プロパ近似
  % [input,c_mem] = dd_strictproper(A,B,Ft,h,N,x0,c_mem,del,tau);
 % むだ時間
 tend = t0+del;t = t0:ts:tend; t = t';
 u = input+zeros(size(m,1))*t;
 [ud,p_mem] = delay(u,h,del,p_mem);
 % 集中部分系
 warning off Control:analysis:LsimStartTime;
 [y,tt,x] = lsim(sys,ud,t,x0);
 t0 = tend; x0 = x(end,:)';
 %
 T = [T;t]; Ud = [Ud;ud]; X = [X;x];
end

% 時間応答の表示
figure(1)
subplot(211)
plot(T,X(:,[1 2]),'-')
legend('\phi','\theta')
ylabel('\phi, \theta [rad]')
grid
subplot(212)
stairs(T,Ud)
xlabel('Time [s]')
ylabel('u [v]')
grid

%  周波数応答の計算 G_zw(s)Delta(s)
w = logspace(-2,4,1000);
% 制御器 (どちらかを選択)
 % プロパ近似
 loop_gain_proper;
 % 厳密プロパ近似
 % loop_gain_strictproper;

% 周波数応答の表示 
figure(2)
semilogx(w,Loop_gain)
xlim([10^(-1) pi/del])
xlabel('Frequency [rad/s]')
ylabel('Gain [dB]')
grid
 
% EOF