% INIZIO DEL PROGRAMMA
%
% tutte le variabili eventualmente presenti in memoria
% sono messe a disposizione con le istruzioni clear e close
%
clear all
close all
%
% Esercitazione n. 13 - Analisi cinematica col 
% metodo delle equazioni di vincolo
%
% Se uso Octave
%
printf('Hello, this is the beginning ! \n');
%
% Se uso MatLab
% display('Hello, this is the beginning !');
%
%  Definisco i valori dei parametri (fissi)
%
l1 = 80
l2 = 20
l3 = 50
l4 = 70
%   Angolo iniziale della manovella
theta2 =  20 * pi / 180;
%
% omega2 = 400 * ( 2 * pi / 60);
% alpha2 = 0;
% =========================================================================
%  Iterazione iniziale
% =========================================================================
%  
%  Definizione delle coordinate di primo tentativo
%
x2 = 40 ; 
y2 = 20 ; 
%
% theta3 (esempio)
%  = 110 per la configurazione nel semipiano y > 0
%  = -70 per la configurazione emissimmatrica
theta3 = 110 *(pi/180); 
%
x3 = 40; 
% y3 (esempio)
% y3 = 60 per la prima y3 =-40 per la seconda
y3 = 60; 
% theta4 (esempio)
% = -80 per la prima = 50 per la seconda
theta4 = -80*(pi/180); 
x4 = 70; 
% y4 (esempio)
% = 80 per la prima -70 per la seconda
y4 = 80; 
%
%  Definizione del vettore u di primo tentativo
%
u(1) = x2;
u(2) = y2;
u(3) = theta3;
u(4) = x3;
u(5) = y3;
u(6) = theta4;
u(7) = x4;
u(8) = y4;
%
% Illustrazione della situazione di partenza
% 
figure(1)
% disegno il telaio
AX = 0;
AY = 0;
DX = 80;
DY = 0;
Xlinea = [AX,DX];
Ylinea = [AY,DY];
plot(Xlinea,Ylinea,'-oy','LineWidth',2);
hold on
% disegno la manovella
A2X = x2 - (l2/2)*cos(theta2);
A2Y = y2 - (l2/2)*sin(theta2);
B2X = x2 + (l2/2)*cos(theta2);
B2Y = y2 + (l2/2)*sin(theta2);
Xlinea = [A2X,B2X];
Ylinea = [A2Y,B2Y];
plot(Xlinea,Ylinea,'-or','LineWidth',1);
% se uso Matlab axis equal;
% se uso Matlab axis([-20 120 -100 120]);
% se uso Octave
axis([-20 120 -100 120],"equal");
grid on;
ylabel(' Asse y del piano');
xlabel(' Asse x del piano');  
title([' Configurazione delle tre aste mobili nella posizione di primo tentativo ']);
set(1,'Color','w');
hold on
% disegno la biella
B3X = x3 - (l3/2)*cos(theta3);
B3Y = y3 - (l3/2)*sin(theta3);
C3X = x3 + (l3/2)*cos(theta3);
C3Y = y3 + (l3/2)*sin(theta3);
Xlinea = [B3X,C3X];
Ylinea = [B3Y,C3Y];
plot(Xlinea,Ylinea,'-or','LineWidth',1);
hold on
% disegno il bilanciere
C4X = x4 - (l4/2)*cos(theta4);
C4Y = y4 - (l4/2)*sin(theta4);
D4X = x4 + (l4/2)*cos(theta4);
D4Y = y4 + (l4/2)*sin(theta4);
Xlinea = [C4X,D4X];
Ylinea = [C4Y,D4Y];
plot(Xlinea,Ylinea,'-or','LineWidth',1);
hold off
%
%  Definisco il vettore dei resti Psi
%
Psi(1) = x2 - (1/2)*l2*cos(theta2);
Psi(2) = y2 - (1/2)*l2*sin(theta2);
Psi(3) = x3 -(l3/2)*cos(theta3) - x2 - (l2/2)*cos(theta2);
Psi(4) = y3 -(l3/2)*sin(theta3) - y2 - (l2/2)*sin(theta2);
Psi(5) = x4 -x3 -(1/2)*l3*cos(theta3) -(1/2)*l4*cos(theta4);
Psi(6) = y4 -y3 -(1/2)*l3*sin(theta3) -(1/2)*l4*sin(theta4);
Psi(7) = x4 - l1 + (1/2)*l4*cos(theta4);
Psi(8) = y4 + (1/2)*l4*sin(theta4);
%
%  Definisco la norma del vettore dei resti
%
Resti(1) = norm(Psi);
%
%  Definisco la matrice jacobiana relativa alle variabili dipendenti
%
PsiU = zeros(8,8);
%
PsiU(1,1) = 1;
%
PsiU(2,2) = 1;
%
PsiU(3,1) = -1;
PsiU(3,3) = (1/2)*l3*sin(theta3);
PsiU(3,4) = 1;
%
PsiU(4,2) = -1;
PsiU(4,3) = -(1/2)*l3*cos(theta3);
PsiU(4,5) = 1;
%
PsiU(5,3) = (1/2)*l3*sin(theta3);
PsiU(5,4) = -1;
PsiU(5,6) = (1/2)*l4*sin(theta4);
PsiU(5,7) = 1;
%
PsiU(6,3) = -(1/2)*l3*cos(theta3);
PsiU(6,5) = -1;
PsiU(6,6) = -(1/2)*l4*cos(theta4);
PsiU(6,8) = 1;
%
PsiU(7,6) = -(1/2)*l4*sin(theta4);
PsiU(7,7) = 1;
%
PsiU(8,6) = (1/2)*l4*cos(theta4);
PsiU(8,8) = 1;
%
%  Calcolo dell'incremento del vettore u con il metodo di Newton Raphson
%
DeltaU = - inv(PsiU) * Psi' ;
%
%
%  INIZIO DEL CICLO DI ITERAZIONE
%
% numero di iterazioni
%
Niteraz = 4
%
for i = 2:Niteraz
    %
    u = u + DeltaU';
    %
    x2     = u(1);
    y2     = u(2);
    theta3 = u(3);
    x3     = u(4);
    y3     = u(5);
    theta4 = u(6);
    x4     = u(7);
    y4     = u(8);
    %
    Psi(1) = x2 - (1/2)*l2*cos(theta2);
    Psi(2) = y2 - (1/2)*l2*sin(theta2);
    Psi(3) = x3 -(l3/2)*cos(theta3) - x2 - (l2/2)*cos(theta2);
    Psi(4) = y3 -(l3/2)*sin(theta3) - y2 - (l2/2)*sin(theta2);
    Psi(5) = x4 -x3 -(1/2)*l3*cos(theta3) -(1/2)*l4*cos(theta4);
    Psi(6) = y4 -y3 -(1/2)*l3*sin(theta3) -(1/2)*l4*sin(theta4);
    Psi(7) = x4 - l1 + (1/2)*l4*cos(theta4);
    Psi(8) = y4 + (1/2)*l4*sin(theta4);
    %
    Resti(i) = norm(Psi);
    % 
    %
    PsiU(3,3) = (1/2)*l3*sin(theta3);
    PsiU(4,3) = -(1/2)*l3*cos(theta3);
    PsiU(5,3) = (1/2)*l3*sin(theta3);
    PsiU(5,6) = (1/2)*l4*sin(theta4);
    PsiU(6,3) = -(1/2)*l3*cos(theta3);
    PsiU(6,6) = -(1/2)*l4*cos(theta4);
    PsiU(7,6) = -(1/2)*l4*sin(theta4);
    PsiU(8,6) = (1/2)*l4*cos(theta4);
    %
    DeltaU = - inv(PsiU) * Psi' ;
end;
%
%
figure(2)
plot(Resti,'-b','LineWidth',1);
axis([0 10 0 0.1]);
ylabel(' norma del vettore dei resti (deve annullarsi)');
xlabel(' Numero di iterazioni');  
title([' Andamento della norma del vettore dei resti ']);
set(2,'Color','w');
%
%
% Illustrazione della situazione di partenza
% 
figure(3)
% disegno il telaio
AX = 0;
AY = 0;
DX = 80;
DY = 0;
Xlinea = [AX,DX];
Ylinea = [AY,DY];
plot(Xlinea,Ylinea,'-oy','LineWidth',2);
hold on
% disegno la manovella
A2X = x2 - (l2/2)*cos(theta2);
A2Y = y2 - (l2/2)*sin(theta2);
B2X = x2 + (l2/2)*cos(theta2);
B2Y = y2 + (l2/2)*sin(theta2);
Xlinea = [A2X,B2X];
Ylinea = [A2Y,B2Y];
plot(Xlinea,Ylinea,'-or','LineWidth',1);
% se uso MatLab axis equal;
% se uso Matlab axis([-20 120 -100 120]);
% se uso Octave
axis([-20 120 -20 120],"equal");
grid on;
ylabel(' Asse y del piano');
xlabel(' Asse x del piano');  
title([' Configurazione delle tre aste mobili nella posizione di fine iterazione ']);
set(3,'Color','w');
hold on
% disegno la biella
B3X = x3 - (l3/2)*cos(theta3);
B3Y = y3 - (l3/2)*sin(theta3);
C3X = x3 + (l3/2)*cos(theta3);
C3Y = y3 + (l3/2)*sin(theta3);
Xlinea = [B3X,C3X];
Ylinea = [B3Y,C3Y];
plot(Xlinea,Ylinea,'-or','LineWidth',1);
hold on
% disegno il bilanciere
C4X = x4 - (l4/2)*cos(theta4);
C4Y = y4 - (l4/2)*sin(theta4);
D4X = x4 + (l4/2)*cos(theta4);
D4Y = y4 + (l4/2)*sin(theta4);
Xlinea = [C4X,D4X];
Ylinea = [C4Y,D4Y];
plot(Xlinea,Ylinea,'-or','LineWidth',1);
%
%  INIZIO DEL CALCOLO DELLE VELOCITA' 
%
% Calcolo del minore dello jacobiano relativo alle variabili
% input v
%
%  inizializzo
%
PsiV = zeros(8,1)
%
% assegno i valori non nulli
PsiV(1,1) = (1/2)*l2*sin(theta2);
PsiV(2,1) = -(1/2)*l2*cos(theta2);
PsiV(3,1) = (1/2)*l2*sin(theta2);
PsiV(4,1) = -(1/2)*l2*cos(theta2);
%
% calcolo della matrice dei rapporti delle velocità
%
R = inv(PsiU) *PsiV;
%
% Ora devo definire il vettore delle velocità VP dei membri
% input: nel caso particolare il vettore ha solo un elemento
%
% assegno la velocità angolare della manovella omega2
VP = zeros(1,1);
omega2 = 400 * (2*pi/60) ; % 400 giri al minuto
VP = omega2
%
% Calcolo del vettore velocità dei membri cedenti (coordinate
%  dipendenti
%
UP = - R * VP;
%
% ASSEGNAZIONE dei valori determinati
%
v2x = UP(1);
v2y = UP(2);
omega3 = UP(3);
v3x = UP(4);
v3y = UP(5);
omega4 = UP(6)
v4x = UP(7);
v4y = UP(8);
%
% scala del disegno ( pos indica la posizione dell'estremo del 
% vettore velocità sul disengo secondo la scala adottata)
%
scalaV = 0.03;
%
posV2x = x2+ scalaV * v2x;
posV2y = y2+ scalaV * v2y;
posV3x = x3 + scalaV*v3x;
posV3y = y3 + scalaV*v3y;
posV4x = x4 + scalaV*v4x;
posV4y = y4 + scalaV*v4y;
%
hold on
Xlinea = [x2,posV2x];
Ylinea = [y2,posV2y];
plot(Xlinea,Ylinea,'-b','LineWidth',3);
%
hold on
Xlinea = [x3,posV3x];
Ylinea = [y3,posV3y];
plot(Xlinea,Ylinea,'-b','LineWidth',3);
%
%
hold on
Xlinea = [x4,posV4x];
Ylinea = [y4,posV4y];
plot(Xlinea,Ylinea,'-b','LineWidth',3);
%
% CALCOLO DELLE ACCELERAZIONI
%
% definizione della matrice PsiUP i cui elementi sono 
% le derivate temporali delgi elementi della PsiU
% NOTA: si può anche ottenere come jacobiano rispetto alle u 
% del vettore ( PsiU * uP) ove uP è il vettore delle derivate
% temporali del vettore u
%
PsiUP = zeros(8,8);
PsiUP(3,3) =  (1/2)*l3*cos(theta3)*omega3;
PsiUP(4,3) = (1/2)*l3*sin(theta3)*omega3;
PsiUP(5,3) = (1/2)*l3*cos(theta3)*omega3
PsiUP(6,3) = (1/2)*l3*sin(theta3)*omega3;
PsiUP(5,6) = (1/2)*l4*cos(theta4)*omega4;
PsiUP(6,6) = (1/2)*l4*sin(theta4)*omega4;
PsiUP(7,6) = -(1/2)*l4*cos(theta4)*omega4;
PsiUP(8,6) = -(1/2)*l4*sin(theta4)*omega4;
%
% Definizione della matrice PsiVP i cui elementi sono
% le deerivate temporali degli elementi della PsiV
%
PsiVP = zeros(8,1);
PsiVP(1) = (1/2)*l2*cos(theta2)*omega2;
PsiVP(2) = (1/2)*l2*sin(theta2)*omega2;
PsiVP(3) = (1/2)*l2*cos(theta2)*omega2;
PsiVP(4) = (1/2)*l2*sin(theta2)*omega2;
%
% Definizione del vettore delle accelerazioni indipendenti
%  (in questo caso questo vettore ha solo un elemento)
%
% assegno l'accelerazione angolare della manovella alpha2
%
VPP = zeros(1,1);
alpha2 =  0   ; 
VPP(1) = alpha2;
%
H1 =  PsiV  * VPP 
H2 = PsiUP * UP 
H3 = PsiVP * VP 
H0 = H1 + H2 + H3;
%
UPP = - inv(PsiU ) *  H0;
%
% Assegnazione delle accelerazioni
%
%
a2x = UPP(1);
a2y = UPP(2);
alpha3 = UPP(3);
a3x = UPP(4);
a3y = UPP(5);
alpha4 = UPP(6)
a4x = UPP(7);
a4y = UPP(8);
%
% Rappresentazione delle accelerazioni sul disegno
%
% scala del disegno ( pos indica la posizione dell'estremo del 
% vettore velocità sul disengo secondo la scala adottata)
%
scalaA = 0.0005;
%
posA2x = x2+ scalaA * a2x;
posA2y = y2+ scalaA * a2y;
posA3x = x3 + scalaA*a3x;
posA3y = y3 + scalaA*a3y;
posA4x = x4 + scalaA*a4x;
posA4y = y4 + scalaA*a4y;
%
hold on
Xlinea = [x2,posA2x];
Ylinea = [y2,posA2y];
plot(Xlinea,Ylinea,'-g','LineWidth',4);
%
hold on
Xlinea = [x3,posA3x];
Ylinea = [y3,posA3y];
plot(Xlinea,Ylinea,'-g','LineWidth',4);
%
%
hold on
Xlinea = [x4,posA4x];
Ylinea = [y4,posA4y];
plot(Xlinea,Ylinea,'-g','LineWidth',4);
%
%
hold off
% Se uso Matlab
% display(' I have done ! ')
% display(' Good bye class ! ')
% Se uso Octave
printf('I have done! \n');
printf('Good bye class ! \n');