Hi there, does anyone know how to help me resolve this issue? My code keeps running on and on. If you can help me figure the issue please :)? And also the plots are not showin

Question

Junior 2022-11-15

0
链接

此问题的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/1852028-hi-there-does-anyone-know-how-to-help-me-resolve-this-issue-my-code-keeps-running-on-and-on-if-yo

评论： Junior 2022-11-17

clear
close all
d2r=pi/180;
L1=0;      %L1 at ground 
L2=0.1;    %L2 =OA link
L3=0.09;   %L3= AB link
L4=0.065;  %L4= BD=CD
L5=0.175   %L5= CE
e=0.07;
d=0.1
f=0.01
s6=0.4            %calculated distance using Trigonometry to find the distance 
epsilon4= 0.6
epsilon3= 0.3
epsilon2= 0
e=0.1;
th20=40*d2r; % guess %initial position
th30=10*d2r
th40=30*d2r
th50=135*d2r
dirac= 65*d2r
q=[40*d2r 10*d2r 30*d2r 135*d2r 0.4]';
omega2=2*pi;
omega3=3*pi;
omega4=4*pi;
omega5=5*pi;
tf=5; h=0.02; t=0:h:tf; N=length(t); % over 5 cycles
qq=zeros(N,5); % to initialize the matrix of results
dqq=zeros(N,5);
ddqq=zeros(N,5);
h = waitbar(0,'Please wait...');
for i=1:N
   theta2=th20+omega2*t(i);
   q=positionfourbar(q,L2,L3,L4,L5,e,d,f,s6);
    qq(i,:)=q';
    
    J=gradfourbar(q,L2,L3,L4);
    dq=J\[0 0 0 0 omega4]';
    dqq(i,:)=dq';
    
    gam=gamfourbar(q,dq,L2,L3,L4);
    ddq=J\gam;
   ddqq(i,:)=ddq';
       waitbar(i/N)
end
close(h)
save fourbar
figure(1)
clf
subplot(2,1,1)
plot(t,qq(:,2)/d2r)
%plot(qq(:,1)/d2r,qq(:,2)/d2r)
ylabel('\theta_3 deg')
grid
subplot(2,1,2)
plot(t,qq(:,3)/d2r)
%plot(qq(:,1)/d2r,qq(:,3))
ylabel('\theta_4 deg')
grid
xlabel('\ittime s')
%xlabel('\theta_2 deg')
figure(2)
clf
subplot(2,1,1)
plot(qq(:,1)/d2r,dqq(:,2))
ylabel('\omega_3  rad/s')
grid
subplot(2,1,2)
plot(qq(:,1)/d2r,dqq(:,3))
ylabel('\omega_4  rad/s')
grid
xlabel('\theta_2 deg')
figure(3)
clf
subplot(2,1,1)
plot(qq(:,1)/d2r,ddqq(:,2))
ylabel('\epsilon_3   rad/s^2')
grid
subplot(2,1,2)
plot(qq(:,1)/d2r,ddqq(:,3))
ylabel('\epsilon_4   rad/s^2')
grid
xlabel('\theta_2 deg')
function Phi=fourbarcrank(q,L2,L3,L4,L5,e,d,f,s6)
L1=0;      %L1 at ground 
L2=0.1;    %L2 =OA link
L3=0.09;   %L3= AB link
L4=0.065;  %L4= BD=CD
L5=0.175   %L5= CE
e=0.07;
d=0.1
f=0.01
s6=0.4            %calculated distance using Trigonometry to find the distance 
epsilon4= 0.6
epsilon3= 0.3
epsilon2= 0
d2r=pi/180
dirac= 65*d2r
q=[40*d2r 10*d2r 30*d2r 135*d2r 0.4]';
omega2=2*pi;
omega3=3*pi;
omega4=4*pi;
omega5=5*pi;
th20=40*d2r; % guess %initial position
th30=10*d2r
th40=30*d2r
th50=135*d2r
tf=5; h=0.02; t=0:h:tf; N=length(t);
i=1:N
theta2=th20+omega2*t(i);
theta3=th30+omega3*t(i);
theta4=th40+omega4*t(i);
theta5=th50+omega5*t(i);
Phi=[L2*cos(theta2)+L3*cos(theta3)-d-L4*cos(theta4+dirac);...
    L2*sin(theta2)+L3*sin(theta3)-e-L4*sin(theta4+dirac);...
    L4*cos(theta4+dirac)+L5*cos(theta5)-s6;...
    L4*sin(theta4+dirac)+L5*sin(theta5)-f;...
    theta4-th40-omega4-epsilon4*t]
end
function J=jacobianfourbar(q,L2,L3,L4,L5,s6)
L1=0;      %L1 at ground 
L2=0.1;    %L2 =OA link
L3=0.09;   %L3= AB link
L4=0.065;  %L4= BD=CD
L5=0.175   %L5= CE
e=0.07;
d=0.1
f=0.01
s6=0.4            %calculated distance using Trigonometry to find the distance 
dirac= 65*pi/180
q=[40*pi/180 10*pi/180 30*pi/180 135*pi/180 0.4]';
omega2=2*pi;
omega3=3*pi;
omega4=4*pi;
omega5=5*pi;
th20=40*pi/180; % guess %initial position
th30=10*pi/180
th40=30*pi/180
th50=135*pi/180
tf=5; h=0.02; t=0:h:tf; N=length(t);
i=1:N
theta2=th20+omega2*t(i);
theta3=th30+omega3*t(i);
theta4=th40+omega4*t(i);
theta5=th50+omega5*t(i);
J=[-L2*sin(q(1)) -L3*sin(q(2)) -L4*sin(q(3)) 0 0;...
    L2*cos(q(1)) L3*cos(q(2)) -L4*cos(q(3)) 0 0;...
    0 0 -L4*sin(q(3)) -L5*sin(q(4)) -1;...
    0 0 L4*sin(q(3)) L5*cos(q(4)) 0;...
    0 0 1 0 0]
end
 
function q=positionfourbar(q,L2,L3,L4,L5,e,d,f,s6)
q=[40*pi/180 10*pi/180 30*pi/180 135*pi/180 0.4]';
L1=0    %L1 at ground 
L2=0.1;    %L2 =OA link
L3=0.09;   %L3= AB link
L4=0.065;  %L4= BD=CD
L5=0.175   %L5= CE
e=0.07;
d=0.1;
f=0.01;
s6=0.4;           %calculated distance using Trigonometry to find the distance 
epsilon= 2.15;
Phi=fourbarcrank(q,L2,L3,L4,L5,e,d,f,s6);
while norm(Phi)>epsilon
    J=jacobianfourbar(q,L2,L3,L4,L5,s6);
    dq=-inv(J)*Phi;
    q=q+dq;
   Phi=fourbarcrank(q,L2,L3,L4,L5,e,d,f,s6);
end
end 
         function gam=gammalast(q,dq,L2,L3,L4,L5,s6)
     q=[40*pi/180 10*pi/180 30*pi/180 135*pi/180 0.4]';
L1=0    %L1 at ground 
L2=0.1;    %L2 =OA link
L3=0.09;   %L3= AB link
L4=0.065;  %L4= BD=CD
L5=0.175   %L5= CE
e=0.07;
d=0.1;
f=0.01;
s6=0.4;
epsilon4= 0.6
epsilon3= 0.3
epsilon2= 0
d2r=pi/180
dirac= 65*d2r
q=[40*d2r 10*d2r 30*d2r 135*d2r 0.4]';
omega2=2*pi;
omega3=3*pi;
omega4=4*pi;
omega5=5*pi;
th20=40*d2r; % guess %initial position
th30=10*d2r
th40=30*d2r
th50=135*d2r
tf=5; h=0.02; t=0:h:tf; N=length(t);
i=1:N
theta2=th20+omega2*t(i);
theta3=th30+omega3*t(i);
theta4=th40+omega4*t(i);
theta5=th50+omega5*t(i);%calculated distance using Trigonometry to find the distance 
epsilon= 2.15;
Phi=[L2*cos(theta2)+L3*cos(theta3)-d-L4*cos(theta4+dirac);...
    L2*sin(theta2)+L3*sin(theta3)-e-L4*sin(theta4+dirac);...
    L4*cos(theta4+dirac)+L5*cos(theta5)-s6;...
    L4*sin(theta4+dirac)+L5*sin(theta5)-f;...
    theta4-th40-omega4-epsilon4*t];
J=[-L2*sin(q(1)) -L3*sin(q(2)) -L4*sin(q(3)) 0 0;...
    L2*cos(q(1)) L3*cos(q(2)) -L4*cos(q(3)) 0 0;...
    0 0 -L4*sin(q(3)) -L5*sin(q(4)) -1;...
    0 0 L4*sin(q(3)) L5*cos(q(4)) 0;...
    0 0 1 0 0];
dq=-inv(J)*Phi;
% Defines the RHS of the acc. eqs (gamma) for our equation
gam=[L2*cos(q(1))*dq(1)^2+L3*cos(q(2))*dq(2)^2-L4*cos(q(3))*dq(3)^2;...
    L2*sin(q(1))*dq(1)^2+L3*sin(q(2))*dq(2)^2-L4*sin(q(3))*dq(3)^3;...
    L4*cos(q(3))*dq(3)^2+L5*cos(q(4))*dq(4)^2;...
    L4*sin(q(3))*dq(3)^2+L5*sin(q(4))*dq(4)^2;]
end