Em sửa lại đoạn code hôm trước, hôm nay mới có máy kiểm tra lại thấy có 1 lỗi nhỏ. Code mới như sau, anh xem hộ em.
Code:
clear all; clc;
hold on; grid on;
t=0:0.1:3.5; % t là thời gian
l1=300*ones(1,length(t)); %Chiều dài khâu 1
l2=300*ones(1,length(t)); %Chiều dài khâu 2
q1=0.4*cos(t);
q2=1*sin(t);
plot(t,q1,'b');
plot(t,q2,'b');
x=l1.*cos(q1)+l2.*cos(q1+q2);
y=l1.*sin(q1)+l2.*sin(q1+q2);
C2=(x.^2+y.^2-l1.^2-l2.^2)/2./l1./l2;
S2=sqrt(abs(1-C2.^2));%nghiệm còn lại là S2=-sqrt(abs(1-C2.^2));
q2=atan2(S2,C2);
S1=y.*(l1+l2.*C2)-x.*l2.*S2;
%Thực tế S1 và C1 còn có mẫu số nữa nhưng nó trùng với mẫu số của C1 nên lúc dùng %atan2 mình bỏ mẫu số đi để tránh trường hợp mẫu số nó gần về 0
C1=y.*l2.*S2+x.*(l1+l2.*C2);
q1=atan2(S1,C1);
plot(t,q1,'r');
plot(t,q2,'r');