Matlab:四杆机构函数

function [theta,omega,alpha]=crank_rocker(theta1,omega1,alpha1,l1,l2,l3,l4)
% 1. 计算从动件的角位移
L=sqrt(l4*l4+l1*l1-2*l1*l4*cos(theta1));
phi=asin((l1./L)*sin(theta1));
beta=acos((-l2*l2+l3*l3+L*L)/(2*l3*L));
if beta<0
    beta=beta+pi;
end
theta3=pi-phi-beta; % theta3 表示杆3转过角度
theta2=asin((l3*sin(theta3)-l1*sin(theta1))/l2);
% theta2 表示杆2转过角度
theta=[theta2;theta3] ;
% 2. 计算从动件的角速度
A=[-l2*sin(theta2), l3*sin(theta3); % 机构从动件的位置参数矩阵
    l2*cos(theta2), -l3*cos(theta3)];
B=[l1*sin(theta1); -l1*cos(theta1)]; % 机构原动件的位置参数列阵
omega=A\(omega1*B); % 机构从动件的速度列阵
omega2=omega(1);
omega3=omega(2);
% 3. 计算从动件的角加速度
A=[-l2*sin(theta2), l3*sin(theta3); l2*cos(theta2), -l3*cos(theta3)];
At=[-omega2*l2*cos(theta2), omega3*l3*cos(theta3); -omega2*l2*sin(theta2), omega3*l3*sin(theta3)];
B=[l1*sin(theta1); -l1*cos(theta1)];
% 机构原动件的位置参数列阵
Bt=[omega1*l1*cos(theta1); omega1*l1*sin(theta1)]; % Bt=dB/dt
alpha=A\(-At*omega+alpha1*B+omega1*Bt); % 机构从动件的加速度列阵

figure(6)
plot(0:0.001:(tend*2),data_axe,'r')
grid on
title('E点X方向加速度'); 
xlabel('时间(s)') 
ylabel('加速度(m/s^2)')

figure(7)
plot(0:0.001:(tend*2),data_aye,'r')
grid on
title('E点Y方向价速度'); 
xlabel('时间(s)') 
ylabel('加速度(m/s^2)')

plot(n1,theta2*du,n1,theta3*du,'k'); 
title('角位移线图'); 
xlabel('曲柄转角 \theta_1 / \circ');
ylabel('角位移 / \circ'); 
grid on; 
text(140,170,'\theta_3'); 
text(140,30,'\theta_2');

plot(n1,omega2,n1,omega3,'k');
title('角速度线图'); 
xlabel('曲柄转角 \theta_1 / \circ');
ylabel('角速度 / rad\cdots^{-1}');
grid on;
text(250,130,'\omega_2');
text(130,165,'\omega_3');

plot(n1,alpha2,n1,alpha3,'k');
title('角加速度线图');
xlabel('曲柄转角 \theta_1 / \circ');
ylabel('角加速度 / rad\cdots^{-2}');
grid on;
text(230,2e4,'\alpha_2');
text(30,7e4,'\alpha_3');