ips49_p.m

来自「基于Fuzzy 理论的 倒立摆 Matlab仿真系统 源代码」· M 代码 · 共 47 行

clear;clc;
%Initial Condition
x(1,:)=[0.1,0,0];
%Simulation Setting
h=0.001;
% Set point r=0;
r=0;


k=1;
for t=h:h:3;        
% The output of Fuzzy Controller 
%fc is a function of Fuzzy Controller
    u(k)=fc49_p(-x(k,1),-x(k,2));  

% Runge-Kutta method for approximating the next state.
%ff denotes the F in chapter 2
    k=k+1;
    k1=h*ff(x(k-1,:),u(k-1),t);
    k2=h*ff(x(k-1,:)+k1/2,u(k-1),t);
    k3=h*ff(x(k-1,:)+k2/2,u(k-1),t);
    k4=h*ff(x(k-1,:)+k3,u(k-1),t);
    x(k,:)=x(k-1,:)+(k1+2*k2+2*k3+k4)/6;
%e saturate at +- pi/2;    
    if x(k,1) > (3*pi)/2
        x(k,1)=(3*pi)/2;
    end;
    if x(k,1)<(-3*pi)/2
        x(k,1)=(-3*pi)/2;
    end;
%e_dot saturate at +- pi/4;    
    if x(k,2) > (3*pi)/8
        x(k,2)=(3*pi)/8;
    end;
    if x(k,2)<-(3*pi)/8
        x(k,2)=-(3*pi)/8;
    end;    
end;

%Plot the curves.
u(k)=u(k-1);
t=0:h:3;
figure(1);
subplot(2,1,1);
plot(t,x(:,1));grid on;title('The gains are g0=10, g1=0.5, h=5 (49 rules)');ylabel('Angular position(rad)');
subplot(2,1,2);
plot(t,u);grid on;ylabel('Input Force(N)');xlabel('Time(sec)');