Initial commit

MATLAB/Cinematica/analisi_posizione_velocita.m

@@ -0,0 +1,99 @@
+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

MATLAB/Cinematica/newtonsis2.m

@@ -0,0 +1,16 @@
+function[xn,k,info,Jk] = newtonsis2(f1 ,f2 ,df11 ,df12 ,df21 ,df22 ,kmax ,tol ,xc)
+info=0;
+F0(1)= f1(xc) ; F0(2)= f2(xc);
+Fk=F0 ;
+
+for k=1:kmax
+    Jk(1,1)= df11(xc) ; Jk(1,2)= df12(xc) ; Jk(2,1)= df21(xc) ; Jk(2,2)= df22(xc);
+    xn=xc-(inv(Jk))*Fk' ;
+    Fk(1)= f1(xn) ; Fk(2)= f2 (xn);
+    if norm( xn -xc)< tol && norm(Fk)< tol
+        return
+    end
+    xc=xn;
+end
+info=1;
+end