Where is the mistake? This code doesn't work.
3 views (last 30 days)
Show older comments
clc;
close all;
clear all;
syms x;
% Four Arm Mechanism
%global LAB LBC LDC LEF LEC LAD LAG LGD LAF
LAB=0.09;%m
LBC=0.4;%m
LDC=0.12;%m
LEF=0.21;%m
LEC=0.25;%m
LAD=0.22;%m
LAG=0.35;%m
LGD=0.22;%m
LAF=0.4;%m
Nmax = 100; % max number of iterations
epsk = [1e-6, 1e-6]; % convergence threshold
tet2o = 135 * pi / 180
tet3o = 20 * pi / 180; % rad, initial value of theta3
tet4o = 320 * pi / 180; % rad, initial value of theta4
eps = [0, 0]; % error vector
% Pre-allocate arrays
th2 = 0:1:360; N=length(th2);
teta2 = zeros(1, N); % theta2 in radians
teta3 = zeros(1, N); % theta3 in radians
teta4 = zeros(1, N); % theta4 in radians
teta5 = zeros(1, N);
tet3 = zeros(1, N); % theta3 in degrees
tet4 = zeros(1, N); % theta4 in degrees
%% wbar = waitbar(0, 'Iteration Processing....'); % progress bar
k = 0; % iteration counter
% Loop through values of theta2
for tet2 = 0:1:360
k = k + 1;
tet2 = pi * tet2 / 180; % Convert to radians
teta2(k) = tet2;
tet1o = pi *60 / 180; % Convert to radians
tet3o = 20 * pi / 180; % Reset initial value of tet3o
tet4o = 320 * pi / 180; % Reset initial value of tet4o
tet5o = 30 * pi / 180;
% Loop through iterations
for iter = 1:Nmax
% Update theta3 and theta4
tet3o = tet3o + eps(1);
tet4o = tet4o + eps(2);
% Define function and Jacobian
f = [(LDC * sin(teta3(k)) + LEF * sin(tet5o) -LEC * sin(teta4(k))); (LDC * cos(teta3(k)) + LEF * cos(tet5o) -LEC * cos(tet4o))];
J = [-LEF * cos(tet5o), -LEC * cos(tet4o);-LEF * sin(tet5o), LEC * sin(tet4o)];
% Compute error and check for convergence
eps = inv(J)*f;
if abs(eps(1)) <= epsk(1) && abs(eps(2)) <= epsk(2)
kiter = iter; % number of iterations to converge
tet3(k) = tet3o * 180 / pi; % deg
teta1(k) = tet1o
teta3(k) = tet3o; % rad
teta4(k) = tet4o; % rad
tet4(k) = teta4(k) * 180 / pi; % deg
break;
end
end
end
%% close(wbar);
fid = fopen('reportfourarm.m', 'w');
fprintf(fid, 'teta2(i) teta3(i) teta4(i)\n');
for i = 1:N
fprintf(fid, '%1.2f %6.1f %6.1f\n', i-1, tet3(i), tet4(i));
end
fclose(fid);
%% kon=input(' Analysis Results: Report | <enter>\n');
%% if kon==0
%% end
t=0:1:N-1;
tet4 = 0:1:360;
tet5 = 0:1:360;
360;
% Plot the trajectory of theta_3 and theta_4
plot(t, tet4, 'r-', t, tet4, 'b-', t, tet3, 'g-');
xlabel('Theta (rad)');
ylabel('Theta (deg)');
legend('Theta_3', 'Theta_4', 'Theta_5');
title('Trajectory of Theta_4, Theta_5,');
3 Comments
Answers (0)
See Also
Categories
Find more on Graphics Object Programming in Help Center and File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!