Erick Ahmed
99 lines
2.7 KiB
Matlab
Executable File
99 lines
2.7 KiB
Matlab
Executable File
clc
|
|
clear
|
|
fprintf('Calcolo degli angoli del cinematismo: Quadrilatero Articolato \n');
|
|
|
|
% dimensione vettori del quadrilatero
|
|
z1=248.56; z2=338.66; z3=282.74; z4=198.8;
|
|
|
|
% angoli statici del quadrilatero (theta3, theta4)
|
|
t3=271; t4=181;
|
|
|
|
% range angoli theta2
|
|
initial_angle = 45;
|
|
final_angle= 90;
|
|
|
|
% Analisi posizione
|
|
for j=1:(final_angle - initial_angle)
|
|
|
|
teta2rad=deg2rad(initial_angle+j);
|
|
x0=[deg2rad(t3); deg2rad(t4)];
|
|
kmax=1000; tol=1e-2;
|
|
|
|
f1=@(x) z2*cos(teta2rad)+z3.*cos(x(1))+z4.*cos(x(2))-z1;
|
|
f2=@(x) z2*sin(teta2rad)+z3.*sin(x(1))+z4.*sin(x(2));
|
|
|
|
df11=@(x) -z3.*sin(x(1));
|
|
df12=@(x) -z4.*sin(x(2));
|
|
df21=@(x) z3.*cos(x(1));
|
|
df22=@(x) z4.*cos(x(2));
|
|
|
|
[x,k,info ] = newtonsis2(f1,f2,df11,df12,df21,df22,kmax,tol,x0);
|
|
xrad=rad2deg(x);
|
|
|
|
if info==0
|
|
fprintf(' θ2 = '); disp(rad2deg(teta2rad));
|
|
fprintf(' θ3 calcolato = '); disp(xrad(1));
|
|
fprintf(' θ4 calcolato = '); disp(xrad(2));
|
|
fprintf(' Numero di iterazioni '); disp(k);
|
|
fprintf(' ------------------------------- \n');
|
|
teta2(j)=rad2deg(teta2rad);
|
|
teta3(j)=xrad(1);
|
|
teta4(j)=xrad(2);
|
|
else
|
|
fprintf(' Numero massimo iterazioni raggiunto ');
|
|
end
|
|
|
|
t=deg2rad(110);
|
|
a=[0; z2*cos(teta2rad+t); z2*cos(teta2rad+t)+z3*cos(x(1)+t); z1*cos(t)];
|
|
b=[0; z2*sin(teta2rad+t); z2*sin(teta2rad+t)+z3*sin(x(1)+t); z1*sin(t)];
|
|
|
|
|
|
plot(a,b,'r','linewidth',1)
|
|
grid on
|
|
grid minor
|
|
axis([-400 200 -200 400])
|
|
hold on
|
|
plot(a,b,'o','color', 'r','markersize',8)
|
|
xlabel('x [mm]')
|
|
ylabel('y [mm]')
|
|
title("Kinematic analysis of the front double wishbone position")
|
|
|
|
end
|
|
|
|
figure(2)
|
|
plot (teta2,teta3);
|
|
grid on
|
|
grid minor
|
|
hold on
|
|
plot (teta2,teta4);
|
|
xlabel('θ2 [°]')
|
|
ylabel('θ3 - θ4 [°]')
|
|
legend("θ3","θ4")
|
|
hold off
|
|
title("θ3 and θ4 variations respective to θ2")
|
|
|
|
% Analisi velocita
|
|
|
|
r=input('Digita 1 per il calcolo della velocità, 2 per uscire: ');
|
|
if r==1
|
|
fprintf('Calcolo delle velocità angolari del cinematismo: Quadrilatero Articolato \n');
|
|
a1=input('Inserisci il valore di θ2 di partenza: ');
|
|
a2=input('inserisci il valore θ2 finale: ');
|
|
t=input('inserisci Δt in secondi: ');
|
|
w2=deg2rad((a2-a1)/t);
|
|
|
|
%cerca la posizione del valore messo nell'input a1 nell'array teta2
|
|
for i=1:45
|
|
if a1==teta2(i)
|
|
i1=i;
|
|
end
|
|
end
|
|
|
|
dftx=[-z2*sin(deg2rad(teta2(i1))); z2*cos(deg2rad(teta2(i1)))];
|
|
dfty=[-z3*sin(deg2rad(teta3(i1))) -z4*sin(deg2rad(teta4(i1))); z3*cos(deg2rad(teta3(i1))) z4*cos(deg2rad(teta4(i1)))];
|
|
w=-inv(dfty)*(dftx*w2);
|
|
fprintf('Velocità angolare [rad/s] w2='); disp(w2);
|
|
fprintf('Velocità angolare [rad/s] w3='); disp(w(1));
|
|
fprintf('Velocità angolare [rad/s] w4='); disp(w(2));
|
|
|
|
end |