%Fuzzy Controller
clear all;close all;

%**************************模糊控制單元*********************************%
a=newfis('fuzz_ls');

f1=1.0;
a=addvar(a,'input','e',[-3*f1,3*f1]);                        % Parameter e
a=addmf(a,'input',1,'NB','zmf',[-3*f1,-1*f1]);
a=addmf(a,'input',1,'NM','trimf',[-3*f1,-2*f1,0]);
a=addmf(a,'input',1,'NS','trimf',[-3*f1,-1*f1,1*f1]);
a=addmf(a,'input',1,'Z','trimf',[-2*f1,0,2*f1]);
a=addmf(a,'input',1,'PS','trimf',[-1*f1,1*f1,3*f1]);
a=addmf(a,'input',1,'PM','trimf',[0,2*f1,3*f1]);
a=addmf(a,'input',1,'PB','smf',[1*f1,3*f1]);

f2=1.0;
a=addvar(a,'input','ec',[-3*f2,3*f2]);                      % Parameter ec
a=addmf(a,'input',2,'NB','zmf',[-3*f2,-1*f2]);
a=addmf(a,'input',2,'NM','trimf',[-3*f2,-2*f2,0]);
a=addmf(a,'input',2,'NS','trimf',[-3*f2,-1*f2,1*f2]);
a=addmf(a,'input',2,'Z','trimf',[-2*f2,0,2*f2]);
a=addmf(a,'input',2,'PS','trimf',[-1*f2,1*f2,3*f2]);
a=addmf(a,'input',2,'PM','trimf',[0,2*f2,3*f2]);
a=addmf(a,'input',2,'PB','smf',[1*f2,3*f2]);

%f3=1.4;
f3=1.0;
a=addvar(a,'output','u',[-3*f3,3*f3]);                      % Parameter u
a=addmf(a,'output',1,'NB','zmf',[-3*f3,-1*f3]);
a=addmf(a,'output',1,'NM','trimf',[-3*f3,-2*f3,0]);
a=addmf(a,'output',1,'NS','trimf',[-3*f3,-1*f3,1*f3]);
a=addmf(a,'output',1,'Z','trimf',[-2*f3,0,2*f3]);
a=addmf(a,'output',1,'PS','trimf',[-1*f3,1*f3,3*f3]);
a=addmf(a,'output',1,'PM','trimf',[0,2*f3,3*f3]);
a=addmf(a,'output',1,'PB','smf',[1*f3,3*f3]);

rulelist=[1 1 7 1 1;                                       % Edit rule base
          1 2 7 1 1;
          1 3 6 1 1;
          1 4 6 1 1;
          1 5 5 1 1;
          1 6 5 1 1;
          1 7 4 1 1;
   
          2 1 7 1 1;
          2 2 6 1 1;
          2 3 6 1 1;
          2 4 5 1 1;
          2 5 5 1 1;
          2 6 4 1 1;
          2 7 3 1 1;
          
          3 1 6 1 1;
          3 2 6 1 1;
          3 3 5 1 1;
          3 4 5 1 1;
          3 5 4 1 1;
          3 6 3 1 1;
          3 7 3 1 1;
          
          4 1 6 1 1;
          4 2 5 1 1;
          4 3 5 1 1;
          4 4 4 1 1;
          4 5 3 1 1;
          4 6 3 1 1;
          4 7 2 1 1;
          
          5 1 5 1 1;
          5 2 5 1 1;
          5 3 4 1 1;
          5 4 3 1 1;
          5 5 3 1 1;
          5 6 2 1 1;
          5 7 2 1 1;
          
          6 1 5 1 1;
          6 2 5 1 1;
          6 3 4 1 1;
          6 4 3 1 1;
          6 5 2 1 1;
          6 6 2 1 1;
          6 7 1 1 1;
       
          7 1 4 1 1;
          7 2 3 1 1;
          7 3 3 1 1;
          7 4 2 1 1;
          7 5 2 1 1;
          7 6 1 1 1;
          7 7 1 1 1];
          
a=addrule(a,rulelist);
%showrule(a)                               % Show fuzzy rule base

a1=setfis(a,'DefuzzMethod','centroid');    % Defuzzy
writefis(a1,'ls');                         % save to fuzzy file "ljk.fis"
                                           % simulated with fuzzy tool
a2=readfis('ls');

%******************* Using Fuzzy Controller****************************%
e1_1=0.0;
ec1_1=0.0;
e2_1=0.0;
ec2_1=0.0;

u1(1)=0.0; 
u2(1)=0.0;

X1(1)=0.0;
X2(1)=0.0;

G=[0.9986 0;0 0.9972];
H=[0.5 0.5;-4 2];
C=[1 0;0 1];

% G=[1 0;0 1];
% H=[0.5 0.5;-2 2];
% C=[1 0;0 1];


yout1(1)=0;
yout2(1)=0;
        
ts=0.5;
%********************Start of Control**********************************%
for k=2:1:600
    T(k)=k*ts;
    
     rin1(k)=0.5;      
     rin2(k)=0.5;

    if k>=200                            %100s時rin1有階躍變化
      rin1(k)=1;   
    end
    if k>=400                            %200s時rin1有階躍變化
      rin2(k)=1;   
    end

error1(k)=yout1(k-1)-rin1(k);            %計算偏差
error2(k)=yout2(k-1)-rin2(k);

e1_1=error1(k);
ec1_1=error1(k)-error1(k-1);

e2_1=error2(k);
ec2_1=error2(k)-error2(k-1);

u1(k)=evalfis([e1_1 ec1_1],a2)/3;     %Using fuzzy inference
u2(k)=evalfis([e2_1 ec2_1],a2)/3;    % Using fuzzy inference

%********************耦合單元********************************************%
X1(k)=G(1,1)*X1(k-1)+G(1,2)*X2(k-1)+H(1,1)*u1(k)+H(1,2)*u2(k);
X2(k)=G(2,1)*X1(k-1)+G(2,2)*X2(k-1)+H(2,1)*u1(k)+H(2,2)*u2(k);

yout1(k)=C(1,1)*X1(k);
yout2(k)=C(2,2)*X2(k);
%************************************************************************%

end
%************************************************************************%
 
%************************************************************************%
%    輸出圖形
figure(1);
subplot 211;
plot(T,rin1,'b',T,g3,':black');
xlabel('Time(second)');ylabel('rin1,yout1');
subplot 212;
plot(T,rin2,'b',T,g4,':black');
xlabel('Time(second)');ylabel('rin2,yout2');
figure(2);
subplot 211;

plot(T,h3,':black');
xlabel('Time(second)');ylabel('u1');
subplot 212;
plot(T,h4,':black');
xlabel('Time(second)');ylabel('u2');
%    結束
%************************************************************************%