Where is the mistake? This code doesn't work.
4 ビュー (過去 30 日間)
古いコメントを表示
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,');
回答 (0 件)
参考
カテゴリ
Help Center および File Exchange で Graphics Object Programming についてさらに検索
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!