Matlab模拟平面连杆机构的运动动画

clc,clear
% 输入Table的参数
AB=0.03;AE=0.07;BC=0.05;CD=0.08;a=0.05;psi0=30;n=400;
% 解析法，建立表达式
syms psi real
Ax=0;Ay=0;
% B的轨迹为以A为中心的圆，利用圆的参数方程求B点坐标
Bx=AB*cos(psi);Bx_Function=matlabFunction(Bx);vBx_Function=matlabFunction(diff(Bx,1)*n);aBx_Function=matlabFunction(diff(Bx,2)*n);
By=AB*sin(psi);By_Function=matlabFunction(By);vBy_Function=matlabFunction(diff(By,1)*n);aBy_Function=matlabFunction(diff(By,2)*n);
% 杆2转角
p2=atan(By/(AE+Bx));p2_Function=matlabFunction(p2);
% C的轨迹为以B为中心的圆，利用圆的参数方程求C点坐标
Cx=Bx+BC*cos(p2);Cx_Function=matlabFunction(Cx);vCx_Function=matlabFunction(diff(Cx,1)*n);aCx_Function=matlabFunction(diff(Cx,2)*n);
Cy=By+BC*sin(p2);Cy_Function=matlabFunction(Cy);vCy_Function=matlabFunction(diff(Cy,1)*n);aCy_Function=matlabFunction(diff(Cy,2)*n);
% 利用勾股定理求D点坐标
Dy=Cy-sqrt(CD^2-(Cx-a).^2);Dy_Function=matlabFunction(Dy);vDy_Function=matlabFunction(diff(Dy,1)*n);aDy_Function=matlabFunction(diff(Dy,2)*n);
Dx=a;Ex=-AE;Ey=0;
% 将各点坐标存入矩阵
JiGouWeiZhi=[Ax Ay;Bx By;Cx Cy;Dx Dy;Ex Ey];JiGouWeiZhi_Function=matlabFunction(JiGouWeiZhi);
% 求杆2和4的角速度和角加速度
w2=diff(p2,1)*n;
aw2=diff(p2,2)*n;
w4=diff(acos((Cx-Dx)/CD),1)*n;w4_Function=matlabFunction(w4);
aw4=diff(w4,1);vw4_Function=matlabFunction(diff(w4*n));aw4_Function=matlabFunction(diff(aw4*n));
% 绘制线图
figure(1)
% D点位移
t=linspace(0,2*pi,1000);
subplot(2,2,1)
plot(t,Dy_Function(t))
title('D滑块位移')
xlabel('杆1转角')
ylabel('D滑块位移 m')
subplot(2,2,2)
plot(t,vDy_Function(t))
title('D滑块速度')
xlabel('杆1转角')
ylabel('D滑块速度 m/min')
subplot(2,2,3)
plot(t,aDy_Function(t))
title('D滑块加速度')
xlabel('杆1转角')
ylabel('D滑块加速度 m/min^2')
subplot(2,2,4)
% 绘制psi0时的机构位置
psi0=psi0/180*pi;
JiGouWeiZhi=JiGouWeiZhi_Function(psi0);
plot(JiGouWeiZhi(1:end-1,1),JiGouWeiZhi(1:end-1,2),'k')
hold on
plot(JiGouWeiZhi(1:end-1,1),JiGouWeiZhi(1:end-1,2),'or')
plot(JiGouWeiZhi([2,5],1),JiGouWeiZhi([2,5],2),'k')
plot(JiGouWeiZhi([2,5],1),JiGouWeiZhi([2,5],2),'or')

% 绘制线图
figure(2)
% 杆4转角
t=linspace(0,2*pi,1000);
subplot(2,2,1)
plot(t,w4_Function(t))
title('D滑块位移')
xlabel('杆1转角')
ylabel('杆4转角 rad')
subplot(2,2,2)
plot(t,vw4_Function(t))
title('D滑块速度')
xlabel('杆1转角')
ylabel('杆4角速度 rad/min')
subplot(2,2,3)
plot(t,aw4_Function(t))
title('D滑块加速度')
xlabel('杆1转角')
ylabel('杆4角加速度 rad/min^2')
subplot(2,2,4)

% 绘制机构动画
for i=0:360
    psi=i/180*pi;
JiGouWeiZhi=JiGouWeiZhi_Function(psi);
plot(JiGouWeiZhi(1:end-1,1),JiGouWeiZhi(1:end-1,2),'k')
hold on
plot(JiGouWeiZhi(1:end-1,1),JiGouWeiZhi(1:end-1,2),'.r','MarkerSize',5)
plot(JiGouWeiZhi([2,5],1),JiGouWeiZhi([2,5],2),'k')
plot(JiGouWeiZhi([2,5],1),JiGouWeiZhi([2,5],2),'.r','MarkerSize',5)
axis equal
axis([-0.3 0.3 -0.3 0.3])
pause(0.001)
hold off
end
% 绘制BC中点轨迹
for i=0:360
    psi=i/180*pi;
    JiGouWeiZhi=JiGouWeiZhi_Function(psi);
    PointInBC(i+1,:)=(JiGouWeiZhi(2,:)+JiGouWeiZhi(3,:))/2;
end
hold on
plot(PointInBC(:,1),PointInBC(:,2))

% 各点速度为
'各点速度为'
v=[0 0;vBx_Function(psi0) vBy_Function(psi0);vCx_Function(psi0) vCy_Function(psi0);0 vDy_Function(psi0);0 0];
'各点加速度为'
a=[0 0;aBx_Function(psi0) aBy_Function(psi0);aCx_Function(psi0) aCy_Function(psi0);0 aDy_Function(psi0);0 0];

% 求载荷
% 滑块D受外力2000N
F5=sign(vDy_Function(psi0))*2000;
% 输入各杆的质量和转动惯量
h=0.01;d=0.001;midu=8000;wSlider=0.05;hSlider=0.02;a=0.02;
m1=AB*h*d*midu;
m2=BC*h*d*midu;
m3=wSlider*hSlider*d*midu;
m4=CD*h*d*midu;
m5=wSlider*hSlider*d*midu;
J1=1/12*m1*(h^2+d^2);
J2=1/12*m2*(h^2+d^2);
J3=1/12*m3*(wSlider^2+hSlider^2);
J4=1/12*m4*(h^2+d^2);
J5=1/12*m5*(wSlider^2+hSlider^2);
% 借助CD杆求C点反力
FCy=-F5+m4*(aCy_Function(psi0)+aDy_Function(psi0))/2;
FCx=(FCy*(JiGouWeiZhi(3,1)-JiGouWeiZhi(4,1))+m4*(aCx_Function(psi0))/2)/(JiGouWeiZhi(3,2)-JiGouWeiZhi(4,2));
% 借助BC杆求B点反力
FBC=-FCy*sin(p2_Function(psi0))+FCx*cos(p2_Function(psi0));
FBCcz=FCy*cos(p2_Function(psi0))-FCx*sin(p2_Function(psi0));
FCB=-FBC;
FCBcz=-FBCcz*(JiGouWeiZhi(2,1)-JiGouWeiZhi(5,1))/(JiGouWeiZhi(3,1)-JiGouWeiZhi(5,1));
FBx=FCB*cos(p2_Function(psi0))+FCBcz*sin(p2_Function(psi0));
FBy=FCB*sin(p2_Function(psi0))+FCBcz*cos(p2_Function(psi0));
% 求转矩
M=FBx*AB*cos(p2_Function(psi0))-FBy*AB*sin(p2_Function(psi0));
'电机转矩为：'
M