%%%%%%%%%%%對模型進行線性化

clear all;
close all;

g=9.8;
m=2;
M=8;
l=0.5;
a=1/(m+M);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%方程一
a1=g/(4/3*l-a*m*l);
A1=[0 1;
    a1 0]
b1=-a/(4/3*l-a*m*l);
B1=[0;b1]
%方程二
x2=200*pi/180;
a2=(g-a*m*l*x2^2)/(4/3*l-a*m*l);
A2=[0 1;
    a2 0]
b2=b1;
B2=[0;b2]
%方程三
x1=15*pi/180;
a3=g/(4/3*l-a*m*l*(cos(x1))^2)
 A3=[0 1;
     a3 0]
 b3=-a*cos(x1)/(4/3*l-a*m*l*(cos(x1))^2)    
 B3=[0;b3]
 %方程4
 x1=15*pi/180;
 x2=200*pi/180;
 a41=g/(4/3*l-a*m*l*(cos(x1))^2)
 a42=-a*m*l*x2*sin(2*x1)*0.5/(4/3*l-a*m*l*(cos(x1))^2)
 A4=[0 1;
     a41 a42]
 b4=b3;
 B4=[0;b4]
 %方程5
 x1=-15*pi/180;
 x2=200*pi/180;
 a51=g/(4/3*l-a*m*l*(cos(x1))^2)
 a52=-a*m*l*x2*sin(2*x1)*0.5/(4/3*l-a*m*l*(cos(x1))^2)
 A5=[0 1;
     a51 a52]
 b5=b3;
 B5=[0;b5]
 
 
 %%%%%%%%%%%%控制程序
 
%Sugeno type fuzzy control for single inverted pendulum
close all;

P=[-4+5.457i;-4-5.457i]  %設置穩定極點
%利用極點配置命令place得出反饋增益F
F1=place(A1,B1,P)
F2=place(A2,B2,P)
F3=place(A3,B3,P)
F4=place(A4,B4,P)
F5=place(A5,B5,P)
%設計反饋模糊控制器，用于控制根據輸入來控制U.輸入為狀態X1,X2,輸出為U
%命名模糊控制器名字為tc，輸入為theta,omega,隸屬度函數取為高斯函數，輸出為U
tc=newfis('tc','sugeno');
tc=addvar(tc,'input','theta',[-2 2]);
tc=addmf(tc,'input',1,'NG','gaussmf',[0.4 -1]);
tc=addmf(tc,'input',1,'ZR','gaussmf',[0.345 0]);
tc=addmf(tc,'input',1,'PO','gaussmf',[0.4 1]);

tc=addvar(tc,'input','omega',[-6 6]);
tc=addmf(tc,'input',2,'NG','gaussmf',[1.5 -4]);
tc=addmf(tc,'input',2,'ZR','gaussmf',[1.5 0]);
tc=addmf(tc,'input',2,'PO','gaussmf',[1.5 4]);

tc=addvar(tc,'output','u',[-600 600]);
tc=addmf(tc,'output',1,'No.1','linear',[F1(1) F1(2) 0 ]);
tc=addmf(tc,'output',1,'No.2','linear',[F2(1) F2(2) 0 ]);
tc=addmf(tc,'output',1,'No.3','linear',[F3(1) F3(2) 0]);
tc=addmf(tc,'output',1,'No.4','linear',[F4(1) F4(2) 0]);
tc=addmf(tc,'output',1,'No.5','linear',[F5(1) F5(2) 0]);
%模糊規則設計，8條
rulelist=[1 1 4 1 1;
          1 2 3 1 1;
          1 3 5 1 1;
          2 1 2 1 1;
          2 2 1 1 1;
          2 3 2 1 1;
          3 1 5 1 1;
          3 2 3 1 1;
          3 3 4 1 1];
tc=addrule(tc,rulelist);
showrule(tc)

%%%%%%%%%%%%%%%%%%%%%%%設計線性化模糊控制器，用于將方程線性化，輸入是X1,X2,U,輸出是X1的導數和X2的導數

%設置模糊控制器名字為model,輸入為theta,omega,u,隸屬度函數都取為高斯函數，輸出為d-thete,d-omega，取的是線性化輸出
model=newfis('model','sugeno');
model=addvar(model,'input','theta',[-2 2]);
model=addmf(model,'input',1,'NG','gaussmf',[0.4 -1]);
model=addmf(model,'input',1,'ZR','gaussmf',[0.345 0]);
model=addmf(model,'input',1,'PO','gaussmf',[0.4 1]);

model=addvar(model,'input','omega',[-6 6]);
model=addmf(model,'input',2,'NG','gaussmf',[1.5 -4]);
model=addmf(model,'input',2,'ZR','gaussmf',[1.5 0]);
model=addmf(model,'input',2,'PO','gaussmf',[1.5 4]);

model=addvar(model,'input','u',[-10 10]);
model=addmf(model,'input',3,'Any','gaussmf',[1.5 -5]);

model=addvar(model,'output','d-theta',[-200 200]);
model=addmf(model,'output',1,'No.1','linear',[0 1 0 0]);
model=addmf(model,'output',1,'No.2','linear',[0 1 0 0]);
model=addmf(model,'output',1,'No.3','linear',[0 1 0 0]);
model=addmf(model,'output',1,'No.4','linear',[0 1 0 0]);
model=addmf(model,'output',1,'No.5','linear',[0 1 0 0]);

model=addvar(model,'output','d_omega',[-200 200]);
model=addmf(model,'output',2,'No.1','linear',[A1(2,1) 0 B1(2) 0]);
model=addmf(model,'output',2,'No.2','linear',[A2(2,1) 0 B2(2) 0]);
model=addmf(model,'output',2,'No.3','linear',[A3(2,1) 0 B3(2) 0]);
model=addmf(model,'output',2,'No.4','linear',[A4(2,1) A4(2,2) B4(2) 0]);
model=addmf(model,'output',2,'No.5','linear',[A5(2,1) A5(2,2) B5(2) 0]);
%模糊規則設計，一共9條規則

rulelist1=[1 1 0 4 4 1 1;
           1 2 0 3 3 1 1;
           1 3 0 5 5 1 1;
           2 1 0 2 2 1 1;
           2 2 0 1 1 1 1;
           2 3 0 2 2 1 1;
           3 1 0 5 5 1 1;
           3 2 0 3 3 1 1;
           3 3 0 4 4 1 1];
model=addrule(model,rulelist1);
showrule(model)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%以下為系統仿真程序
ts=0.020;  %仿真間隔時間
x=[0.20 0.1];   %初始狀態



for k=1:1:100
    time(k)=k*ts;
       
    u(k)=(-1)*evalfis([x(1),x(2)],tc);    %使用反饋模糊控制器tc
   
    
    k0=evalfis([x(1),x(2),u(k)],model);    %使用線性化模糊控制器model，
    x=x+ts*k0;   %由于輸出X1,X2的微分，所以X1,X2 的結果只需用間隔時間乘以X的微分，再加上初值即可
    
    y1(k)=x(1);   %設置輸出為狀態
    y2(k)=x(2);
end
figure(1);
subplot(211);
plot(time,y1),grid on;
xlabel('time(s)'),ylabel('Angle');
subplot(212);
plot(time,y2),grid on;
xlabel('time(s)'),ylabel('Angle rate');

figure(2);
plot(time,u),grid on;
xlabel('time(s)'),ylabel('controller output');

figure(3);
plotmf(tc,'input',1);
figure(4);
plotmf(tc,'input',2);

showrule(tc);
showrule(model);