%%%%%%          極配置法           %%%%%
%    倒立振子の制御シミュレーション    %

%初期化
clear;
clg;

% シミュレーションを行う時間
t=[0:0.001:10];

%---------------------------------------
%	パラメータ設定
%---------------------------------------
% 質量(振子)[kg]
m=0.02;
% 質量(台車)[kg]
M=1;
% 振り子の長さ[m]
l=0.1;

% 重力加速度
g=9.8;

% 状態方程式
A=[	0			1 0 0
	3*(m+M)*g/(l*(m+4*M))	0 0 0
	0			0 0 1
	-3*m*g/(M+4*M)		0 0 0];

B=[	0
	-3/(l*(m+4*M))
	0
	4/(m+4*M)];

C=[	1 0 0 0
	0 0 0 0
	0 0 1 0
	0 0 0 0];

D=[	0
	0
	0
	0];

%---------------------------------------
%	極の配置
%	2通りのやり方で書いてある。
%	どちらかの注釈(%)を外して使う。
%	極の値を直接指定する方法と、
%	固有振動数と減衰比を指定する事で
%	指定する方法。
%---------------------------------------
% 直接指定
P=[  -3+2.75j -3-2.75j -4+3.5j -4-3.5j];

% 固有振動数と減衰比による指定。
% 減衰比が1の場合は虚数部は無くなるので
% 実数部のみ指定する事。
%P(1)=3 * exp(j*(pi - acos(0.275)));
%P(2)=3 * exp(j*(pi + acos(0.275)));
%P(3)=4 * exp(j*(pi - acos(0.3)));
%P(4)=4 * exp(j*(pi + acos(0.3)));

%===========================
% フィードバックゲインの計算
%===========================
k=polplace(A,B,P)
%k=place(A,B,P)

%===============
% インパルス応答
%===============
y_p=impulse(A,B,C,D,1,t);
y_c=impulse(A-B*k, B, [C; -k], [D; 0], 1, t);

y_p(:,1)=180/pi*y_p(:,1);
y_c(:,1)=180/pi*y_c(:,1);

u=y_c(:,5);

%=================
% ゼロ点－極の表示
%=================
[num_p,den_p]=ss2tf(A,B,C,D)
[num_c,den_c]=ss2tf(A-B*k,B,C,D)

[pole_pa, zero_pa]=pzmap(num_p(1,:),den_p)
[pole_pd, zero_pd]=pzmap(num_p(3,:),den_p)
[pole_ca, zero_ca]=pzmap(num_c(1,:),den_c)
[pole_cd, zero_cd]=pzmap(num_c(3,:),den_c)

%---------------------------------------
%	グラフの表示
%---------------------------------------

% 極とゼロ点の表示
% パッシブは青、コントロール時は赤で表示
% 極をo,ゼロ点をx,+で表示
figure(1)
plot(real(pole_pa),imag(pole_pa),'bo', real(pole_ca),imag(pole_ca),'ro', real(zero_pa),imag(zero_pa),'bx', real(zero_pd),imag(zero_pd),'b+', real(zero_ca),imag(zero_ca),'rx', real(zero_cd),imag(zero_cd),'r+')
grid
axis([-10 10 -4 4]);
legend('Pole(no cntrol)','Pole(control)','Zero(no cntrol argument)','Zero(no cntrol displacement)','Zero(control argument)','Zero(control displacement)',0)
xlabel('Real Axis')
ylabel('image Axis')
title('Pole-Zero Map')


% 時間応答を表示
% パッシブは青、コントロール時は赤で表示
% 角度θは実線、変位xは破線
figure(2)
subplot(3,1,1)
plot(t, y_p(:,1), 'b-', t, y_c(:,1), 'r-')
grid
axis([0 10 -70 70]);
legend('no cntrol argument','control argument',0)
xlabel('time[sec]')
ylabel('Argument[deg]')

subplot(3,1,2)
plot(t, y_p(:,3), 'b--',  t, y_c(:,3), 'r--')
grid
axis([0 10 -1.5 1.5]);
legend('no cntrol displacement','control displacement',0)
xlabel('time[sec]')
ylabel('Displacement[m]')

subplot(3,1,3)
plot(t, y_c(:,5), 'k:', t, u, 'r-')
grid
%axis([0 10 -2 2]);
xlabel('time[sec]')
ylabel('Control input f')

