%% 仅仅试一下双闭环PID 调试倒立摆
clc;
clear all;
%close all;
%时间变量
tStep = 0.025; %采样时间0.025s
tFinal = 10;
%设定值
theta_last = 0;
x_f_last = 0;
%存放倒立摆数据 第一行是角度,第四行是位移
%初始值 【角度,角速度,位移,线速度,力】
x0=[0,0,1,0,0];
wc(1)=x0(1)*180/pi;
wx(1)=x0(3);
tt = (0:tStep:tFinal);
wx = (0:tStep:tFinal);
wc = (0:tStep:tFinal);
for tp = tStep:tStep:tFinal
%模糊控制
[t,y]=ode45(@in_pendulum_zyc,[tp-tStep,tp],x0);
theta = y(end,1);
x_f = y(end,3);
theta_v = (theta - theta_last)/tStep;
x_f_v = (x_f - x_f_last)/tStep;
%SIRM
F = fuzzy_control_zyc([theta,theta_v,x_f,x_f_v]);
x0 = [y(end,1:4),F];
theta_last = theta;
x_f_last = x_f;
wc(int32(tp/tStep)+1) = theta*180/pi; %画图数据
wx(int32(tp/tStep)+1) = x_f;
end
figure
plotyy(tt,wc,tt,wx);
% subplot(211)
% plot(tt,wc,'r-','LineWidth',2);
% axis([0 tFinal -30 30]); % 设置坐标轴在指定的区间
% ylabel('angle');
% title('angle');
%
% subplot(212)
% plot(tt,wx,'b-','LineWidth',2);
% %axis([0 tFinal -1 1]); % 设置坐标轴在指定的区间
% ylabel('x');
% title('position');
评论0