\n\tclc;clear;close
\nBX=5; BY=15; %\u8d28\u91cf\u7403\u521d\u59cb\u5750\u6807
\nCX0=[-3,3]; CY0=[1.5,1.5]; %\u5c0f\u8f66\u8f66\u4f53
\nMb=1; Mc=10; g=9.8; %\u5c0f\u7403\u8d28\u91cf\/\u5c0f\u8f66\u8d28\u91cf\/\u91cd\u529b\u52a0\u901f\u5ea6
\nCX=mean(CX0)+BX; CY=mean(CY0); %\u5c0f\u8f66\u4e0e\u8fde\u6746\u94fe\u63a5\u4f4d\u7f6e\uff08\u4e5f\u5c31\u662f\u5c0f\u8f66\u4f4d\u7f6e\uff09
\nCXp=CX; %\u5907\u4efd\u4e0a\u4e00\u65f6\u523b\u5c0f\u8f66\u4f4d\u7f6e
\nS0=((BX-CX)^2+(BY-CY)^2)^0.5; %\u8fde\u6746\u957f\u5ea6
\nVbx0=0; Vby0=0; %\u521d\u59cb\u5316\u5c0f\u7403\u901f\u5ea6
\nVcx0=0; Vcy0=0; %\u521d\u59cb\u5316\u5c0f\u8f66\u901f\u5ea6
\nKs=10000; %\u8fde\u6746\u5f39\u6027\u6a21\u91cf
\nKv=100; %\u5c0f\u8f66\u8fd0\u52a8\u963b\u5c3c
\nKp1=2000; Kp2=40000; %\u6bd4\u4f8b\u63a7\u5236\u7cfb\u6570\uff1a1\u4e3a\u5c0f\u7403\u5e73\u8861\uff0c2\u4e3a\u63a7\u5236\u5c0f\u7403\u4f4d\u7f6e
\nKi1=3000; Ki2=3000; %\u79ef\u5206\u63a7\u5236\u7cfb\u6570
\nKd1=500; Kd2=200; %\u5fae\u5206\u5206\u63a7\u5236\u7cfb\u6570
\nintgerX1=0; intgerX2=0; %\u79ef\u5206\u7a7a\u95f4
\ndt=0.01; %\u8fed\u4ee3\u65f6\u95f4\u6b65\u957f<\/p>\n

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\n% \u4e3b\u5faa\u73af
\n%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\nfor t=0:dt:500<\/p>\n

S=((BX-CX)^2+(BY-CY)^2)^0.5; %\u8fde\u6746\u5b9e\u65f6\u957f\u5ea6
\nFs=Ks*(S-S0); %\u8fde\u63a5\u529b
\nVx=(BX-CX)\/S; Vy=(BY-CY)\/S; %\u8fde\u63a5\u529b\u5411\u91cf<\/p>\n

%% \u5bf9\u5c0f\u7403 \u8ba1\u7b97\u53d7\u529b\u548c\u4f4d\u7f6e
\nFbx=-Fs*Vx+1*(1-(mod(t,5)==0))-1*(1-(mod(t,10)==0)); %\u5728x\u65b9\u5411\u52a0\u5e72\u6270
\nFby=-Fs*Vy-Mb*g; %\u8ba1\u7b97Y\u65b9\u5411\u53d7\u529b
\nVbx=Vbx0+Fbx\/Mb*dt; Vby=Vby0+Fby\/Mb*dt; %\u8ba1\u5c0f\u7403\u5b9e\u65f6\u901f\u5ea6
\nBX=BX+Vbx*dt; BY=BY+Vby*dt; %\u66f4\u65b0\u5c0f\u7403\u5b9e\u65f6\u4f4d\u79fb
\nVbx0=Vbx; Vby0=Vby; %\u5907\u4efd\u5c0f\u7403\u901f\u5ea6
\nBXp=BX; BYp=BY; %\u5907\u4efd\u5c0f\u7403\u4f4d\u7f6e\u6570\u636e<\/p>\n

%% \u5bf9\u5c0f\u8f66 \u52a0PID\u63a7\u5236%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\nE1=(BX-CX); E1p=(BXp-CXp); %\u5c0f\u7403\u5e73\u8861\u4f4d\u7f6e\u8bef\u5dee\u51fd\u6570\uff08\u5f53\u524d\u65f6\u523b\u548c\u4e0a\u4e00\u65f6\u523b\uff09
\nE2=0.05*sign(BX)*(1-exp(-abs(BX))); %\u5c0f\u7403\u4f4d\u7f6e\u5b9a\u4f4d\u8bef\u5dee\u51fd\u6570\uff08\u5f53\u524d\u65f6\u523b\uff09
\nE2p=0.05*sign(BXp)*(1-exp(-abs(BXp)));%\u5c0f\u7403\u4f4d\u7f6e\u5b9a\u4f4d\u8bef\u5dee\u51fd\u6570\uff08\u4e0a\u4e00\u65f6\u523b\uff09
\nintgerX1=intgerX1+E1*dt; %\u5bf9\u8bef\u5dee\u51fd\u6570E1\u79ef\u5206
\nintgerX2=intgerX2+E2*dt; %\u5bf9\u8bef\u5dee\u51fd\u6570E2\u79ef\u5206
\nFp=Kp1*E1+Kp2*E2; %\u6bd4\u4f8b\u63a7\u5236\u5206\u91cf
\nFi=Ki1*intgerX1+Ki2*intgerX2; %\u79ef\u5206\u63a7\u5236\u5206\u91cf
\nFd=Kd1*(E1-E1p)\/dt+Kd2*(E2-E2p)\/dt; %\u5fae\u5206\u63a7\u5236\u5206\u91cf<\/p>\n

%%%
\nFcx= Fs*Vx-Kv*Vcx0+Fp+Fi+Fd; %\u66f4\u65b0\u5c0f\u8f66\u53d7\u529b\u2014\u2014X\u65b9\u5411
\nVcx=Vcx0+Fcx\/Mc*dt; %\u66f4\u65b0\u5c0f\u8f66\u901f\u5ea6\u2014\u2014X\u65b9\u5411
\nCX=CX+Vcx*dt; %\u66f4\u65b0\u5c0f\u8f66\u4f4d\u7f6e\u2014\u2014X\u65b9\u5411
\nVcx0=Vcx; %\u5907\u4efd\u5c0f\u8f66\u901f\u5ea6\u2014\u2014X\u65b9\u5411
\nCXp=CX; %\u5907\u4efd\u5c0f\u8f66\u4f4d\u7f6e \u2014\u2014X\u65b9\u5411
\n%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%<\/p>\n

%%%%%%%%%%%%%%%%\u540e\u5904\u7406%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\nif mod(t,1)==0
\n%%%%\u4e0b\u9762\u4e3a\u5c0f\u8f66\u8fd0\u52a8\u56fe%%%%%%%%%%
\n% clf
\n% hold on
\n% set(gcf,'color',[1 1 1])
\n% line(CX0+CX,CY0,'linewidth',20)
\n% plot(0,-0.5,'k*');
\n% plot(mean(CX0)+CX-(CX0(2)-CX0(1))\/4,CY(1)\/3,'ko','markersize',10,'linewidth',5)
\n% plot(mean(CX0)+CX+(CX0(2)-CX0(1))\/4,CY(1)\/3,'ko','markersize',10,'linewidth',5)
\n% plot(BX,BY,'ro','markersize',20,'MarkerFaceColor',[1 0 0])
\n% line([mean(CX0)+CX,BX],[mean(CY0),BY],'linewidth',3,'color',[1 0 0])
\n% line([-20 20],[-0.5 -0.5],'linewidth',1)
\n% axis off
\n% axis equal
\n% text(-1,-2.5,['t=',num2str(t),'s',' BX=',num2str(BX)],'fontsize',12)
\n% axis([-20,20, -5, 20]);
\n% hold off<\/p>\n

%%%%\u4e0b\u9762\u4e3a\u5c0f\u7403X\u5750\u6807\u53d8\u5316\u60c5\u51b5%%%%%%%%%%
\nhold on
\nplot(t,-BX,'r.');plot(t,0,'k.-');
\nhold off
\ndrawnow<\/p>\n

%
\n% %%%%%\u8f93\u51fa\u52a8\u753bGIF
\n% frame = getframe(gcf); % \u83b7\u5f97\u5f53\u524d\u753b\u56fe\u7a97\u53e3h
\n% im = frame2im(frame);
\n% [imind,cm] = rgb2ind(im,256); %\u56fe\u7247\u989c\u8272\u8f6c\u6362256
\n% filename = ['Output',num2str(1),'.gif'];%\u56fe\u7247\u540d\u79f0
\n% if t == 0
\n% imwrite(imind,cm,filename,'gif', 'Loopcount',inf,'DelayTime',0.1);
\n% %\u6709\u4e00\u4e9b\u53c2\u6570\u7f51\u4e0a\u6709\u8bf4\u660e delaytime gif\u64ad\u653e\u65f6\u95f4\u5dee
\n% else
\n% imwrite(imind,cm,filename,'gif','WriteMode','append','DelayTime',0.1);
\n% end
\nend %%%\u540e\u5904\u7406\u7ed3\u675f%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%<\/p>\n

% \u63a7\u5236\u5931\u8d25\u7ec8\u6b62\u6761\u4ef6
\nif BY<mean(CY0) || (BX>20) || (BX<-20)
\n t, break;
\nend<\/p>\n

\n\t\t\n\t<\/div><\/p>\n","protected":false},"excerpt":{"rendered":"