clc;clear;close
BX=5; BY=15; %质量球初始坐标
CX0=[-3,3]; CY0=[1.5,1.5]; %小车车体
Mb=1; Mc=10; g=9.8; %小球质量/小车质量/重力加速度
CX=mean(CX0)+BX; CY=mean(CY0); %小车与连杆链接位置（也就是小车位置）
CXp=CX; %备份上一时刻小车位置
S0=((BX-CX)^2+(BY-CY)^2)^0.5; %连杆长度
Vbx0=0; Vby0=0; %初始化小球速度
Vcx0=0; Vcy0=0; %初始化小车速度
Ks=10000; %连杆弹性模量
Kv=100; %小车运动阻尼
Kp1=2000; Kp2=40000; %比例控制系数：1为小球平衡，2为控制小球位置
Ki1=3000; Ki2=3000; %积分控制系数
Kd1=500; Kd2=200; %微分分控制系数
intgerX1=0; intgerX2=0; %积分空间
dt=0.01; %迭代时间步长
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 主循环
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for t=0:dt:500
S=((BX-CX)^2+(BY-CY)^2)^0.5; %连杆实时长度
Fs=Ks*(S-S0); %连接力
Vx=(BX-CX)/S; Vy=(BY-CY)/S; %连接力向量
%% 对小球 计算受力和位置
Fbx=-Fs*Vx+1*(1-(mod(t,5)==0))-1*(1-(mod(t,10)==0)); %在x方向加干扰
Fby=-Fs*Vy-Mb*g; %计算Y方向受力
Vbx=Vbx0+Fbx/Mb*dt; Vby=Vby0+Fby/Mb*dt; %计小球实时速度
BX=BX+Vbx*dt; BY=BY+Vby*dt; %更新小球实时位移
Vbx0=Vbx; Vby0=Vby; %备份小球速度
BXp=BX; BYp=BY; %备份小球位置数据
%% 对小车 加PID控制%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
E1=(BX-CX); E1p=(BXp-CXp); %小球平衡位置误差函数（当前时刻和上一时刻）
E2=0.05*sign(BX)*(1-exp(-abs(BX))); %小球位置定位误差函数（当前时刻）
E2p=0.05*sign(BXp)*(1-exp(-abs(BXp)));%小球位置定位误差函数（上一时刻）
intgerX1=intgerX1+E1*dt; %对误差函数E1积分
intgerX2=intgerX2+E2*dt; %对误差函数E2积分
Fp=Kp1*E1+Kp2*E2; %比例控制分量
Fi=Ki1*intgerX1+Ki2*intgerX2; %积分控制分量
Fd=Kd1*(E1-E1p)/dt+Kd2*(E2-E2p)/dt; %微分控制分量
%%%
Fcx= Fs*Vx-Kv*Vcx0+Fp+Fi+Fd; %更新小车受力——X方向
Vcx=Vcx0+Fcx/Mc*dt; %更新小车速度——X方向
CX=CX+Vcx*dt; %更新小车位置——X方向
Vcx0=Vcx; %备份小车速度——X方向
CXp=CX; %备份小车位置 ——X方向
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%后处理%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if mod(t,1)==0
%%%%下面为小车运动图%%%%%%%%%%
% clf
% hold on
% set(gcf,’color’,[1 1 1])
% line(CX0+CX,CY0,’linewidth’,20)
% plot(0,-0.5,’k*’);
% plot(mean(CX0)+CX-(CX0(2)-CX0(1))/4,CY(1)/3,’ko’,’markersize’,10,’linewidth’,5)
% plot(mean(CX0)+CX+(CX0(2)-CX0(1))/4,CY(1)/3,’ko’,’markersize’,10,’linewidth’,5)
% plot(BX,BY,’ro’,’markersize’,20,’MarkerFaceColor’,[1 0 0])
% line([mean(CX0)+CX,BX],[mean(CY0),BY],’linewidth’,3,’color’,[1 0 0])
% line([-20 20],[-0.5 -0.5],’linewidth’,1)
% axis off
% axis equal
% text(-1,-2.5,[‘t=’,num2str(t),’s’,’ BX=’,num2str(BX)],’fontsize’,12)
% axis([-20,20, -5, 20]);
% hold off
%%%%下面为小球X坐标变化情况%%%%%%%%%%
hold on
plot(t,-BX,’r.’);plot(t,0,’k.-‘);
hold off
drawnow
%
% %%%%%输出动画GIF
% frame = getframe(gcf); % 获得当前画图窗口h
% im = frame2im(frame);
% [imind,cm] = rgb2ind(im,256); %图片颜色转换256
% filename = [‘Output’,num2str(1),’.gif’];%图片名称
% if t == 0
% imwrite(imind,cm,filename,’gif’, ‘Loopcount’,inf,’DelayTime’,0.1);
% %有一些参数网上有说明 delaytime gif播放时间差
% else
% imwrite(imind,cm,filename,’gif’,’WriteMode’,’append’,’DelayTime’,0.1);
% end
end %%%后处理结束%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 控制失败终止条件
if BY<mean(CY0) || (BX>20) || (BX<-20)
t, break;
end
end