%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%         角度から微分器を使って角速度を導出
%                 2006.4.21
%                                           kizawa
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear

%J_RPEN K design 
% Graphics data %
wb=400;
w=logspace(-3, 3, wb);   %(2, 6, wb);
%---   状態変数 [te       alpa    d_te          d_alpa      ]'
%              [アーム角  振子角  アームの角速度  振子の角速度]

A = [0    0        1  0
     0    0        0  1
     0  -145   -0.94  0
     0    96.2  0.42  0];
B = [0 0 8.15  -3.68]';
   
C = [1 0 0 0
     0 1 0 0
     0 0 1 0
     0 0 0 1];
D = [0 0 0 0]';

sys0=ss(A,B,C,D);

[mag_ ,phase_ ]=bode(sys0,w);
for i=1:wb
     mag_sys0(i,1)=mag_(1,1,i);
     mag_sys0(i,2)=mag_(2,1,i);
     mag_sys0(i,3)=mag_(3,1,i);
     mag_sys0(i,4)=mag_(4,1,i);
     phase_sys0(i,1)=phase_(1,1,i);
end

figure(1)
semilogx( w/(2*pi),20*log10(mag_sys0(:,1)) ,w/(2*pi),20*log10(mag_sys0(:,2)),.......
          w/(2*pi),20*log10(mag_sys0(:,3)) ,w/(2*pi),20*log10(mag_sys0(:,4)));

axis([0.001 100 -60 100])
title('周波数応答','fontsize',10)
xlabel('Frequency(Hz)','fontsize',10)
ylabel('Gain(dB)','fontsize',9)
legend('\theta','\alpha','d\theta','d\alpha',1)
grid on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%---------入力からアーム角への伝達関数の作成------------
C1 = [1 0 0 0];
D1=[0];
sys1=ss(A,B,C1,D1);
disp('入力からアーム角への伝達関数は')
G1= tf(sys1) 
%---------入力からアームの角速度への伝達関数の作成------------
C3 = [0 0 1 0];
D3=[0];
sys3=ss(A,B,C3,D3);
disp('入力からアーム角速度への伝達関数は')
G3=tf(sys3)
[mag_G3_ ,phase_G3_ ]=bode(G3,w);
for i=1:wb
     mag_G3(i,1)=mag_G3_(1,1,i);
end 
%-------微分器----------
disp('アーム角からアーム角速度を求めるための微分器')
G5= tf([50  0],[1  50])         %G5=(50s)/(s+50) 　　変更
[mag_G5_ ,phase_G5_ ]=bode(G5,w);
for i=1:wb
     mag_G5(i,1)=mag_G5_(1,1,i);
end 

%-------微分器 * G1 ====> G3 or G3a ----------
disp('微分器G5 * G1 ====> G3a')
G3a= G5 * G1  

[mag_G3a_ ,phase_G3a_ ]=bode(G3a,w);
for i=1:wb
     mag_G3a(i,1)=mag_G3a_(1,1,i);
end 
figure(2)
semilogx( w/(2*pi),20*log10(mag_G5), w/(2*pi),20*log10(mag_G3), w/(2*pi),20*log10(mag_G3a) )
axis([0.001 100 -60 100])
title('微分器の設計','fontsize',10)
xlabel('Frequency(Hz)','fontsize',10)
ylabel('Gain(dB)','fontsize',9)
legend('微分器','アーム角速度','微分器*アーム角',1)
grid on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%---------入力から振子角への伝達関数の作成------------
C2 = [0 1 0 0];
D2=[0];
sys2=ss(A,B,C2,D2);
disp('入力から振子角への伝達関数は')
G2= tf(sys2) 
%---------入力からアームの角速度への伝達関数の作成------------
C4 = [0 0 0 1];
D4=[0];
sys4=ss(A,B,C4,D4);
disp('入力から振子角速度への伝達関数は')
G4=tf(sys4)
[mag_G4_ ,phase_G4_ ]=bode(G4,w);
for i=1:wb
     mag_G4(i,1)=mag_G4_(1,1,i);
end 
%-------微分器----------
disp('振子角から振子角速度を求めるための微分器')
G6= tf([30  0],[1  30])         %G6=(30s)/(s+30) 　　変更
[mag_G6_ ,phase_G6_ ]=bode(G6,w);
for i=1:wb
     mag_G6(i,1)=mag_G6_(1,1,i);
end 

%-------微分器(G6) * G2 ====> G4 or G4a ----------
disp('微分器(G6) * G2 ====> G4a')
G4a= G6 * G2  

[mag_G4a_ ,phase_G4a_ ]=bode(G4a,w);
for i=1:wb
     mag_G4a(i,1)=mag_G4a_(1,1,i);
end 
figure(3)
semilogx( w/(2*pi),20*log10(mag_G6), w/(2*pi),20*log10(mag_G4), w/(2*pi),20*log10(mag_G4a) )
axis([0.001 100 -60 100])
title('微分器の設計','fontsize',10)
xlabel('Frequency(Hz)','fontsize',10)
ylabel('Gain(dB)','fontsize',9)
legend('微分器','振子角速度','微分器*振子角',1)
grid on