% analisi di velocità del quadrilatero articolato con coppia prismatica
% rispetto ad "analisi_cinematica_accelerazione" viene aggiunto un blocco per 
% l'analisi dinamica

clear 
clf

q_p=13;           % velocità di rotazione della manovella AC [rad/sec] 

% dati geometrici
z1=1;
z4=15;
z3=4;
z5=5;              % posizione del baricentro del corpo 2
chi=40*pi/180;     % valore dato in gradi

% dati delle MASSE e MOMENTI D'INERZIA *******************************
m1=2;              % kg
m2=2.3;
IA=1/2*m1*z1^2;            % kg*m^2     % momento d'inerzia di una sbarra rispetto a un estremo
I2=1/12*m2*(2*z5)^2;                    % momento d'inerzia di una sbarra rispetto al punto medio (si ipotizza che la lunghezza sia 2*z5)


% posizioni delle coordinate indipendenti
vet_q=0:0.1:2*pi;

for count=1:length(vet_q)
    
    % si aggiorna il valore di q
    q=vet_q(count);
    
    % calcolo delle coordinate di manovella
    C=z1*[cos(q),sin(q)]';

    % solizione della diade RPR
    B=[z4,0]';
    HB=z3*sin(chi);
    HD=z3*cos(chi);
    zt=norm(B-C);
    phit=atan2(B(2)-C(2),B(1)-C(1));
    alpha=asin(HB/zt);
    phi2=phit+alpha;
    z2=zt*cos(alpha)-HD;

    % posizione del pattino rispetto al sistema di riferimento
    D=C+z2*[cos(phi2),sin(phi2)]';


    % ANALISI DI VELOCITA' *******************************************************
    phi3=phi2+pi-chi;
    J=[ cos(phi2)       -z2*sin(phi2)+z3*sin(phi3);...
        sin(phi2)       z2*cos(phi2)-z3*cos(phi3)];     % Jacoiano
    
    A=[-z1*sin(q);z1*cos(q)];                           % termine in A (J*x_p+A*q_p=0) 
    
    x_p=-inv(J)*A*q_p;                   % soluzione del sistema
    
    vet_z2_p(count)=x_p(1);              % salvataggio delle velocità in 2 vettore
    vet_phi2_p(count)=x_p(2);
    % ****************************************************************************
    
    
    % ANALISI DI ACCELERAZIONE *******************************************************
    
    J_p=[-sin(phi2)*x_p(2)   -x_p(1)*sin(phi2)-z2*cos(phi2)*x_p(2)+z3*cos(phi3)*x_p(2);...
        cos(phi2)*x_p(2)    x_p(1)*cos(phi2)-z2*sin(phi2)*x_p(2)+z3*sin(phi3)*x_p(2)];     % derivata dello Jacoiano
    
    A_p=-z1*q_p*[cos(q);sin(q)];                           % derivata del termine in A (J*x_p+A*q_p=0) 
    
    x_pp=-inv(J)*(J_p*x_p+A_p*q_p);                   % soluzione del sistema J_p*x_p+J*x_pp+A_p*q_p=0 
                                                      %(manca il termine A*q_pp dato che q_pp=0 (la manovella ruota a vel costante))
    
    vet_z2_pp(count)=x_pp(1);              % salvataggio delle accelerazioni in 2 vettore
    vet_phi2_pp(count)=x_pp(2);
    % ****************************************************************************   
    
    % ACCELERAIONI DEI BARICENTRI DEI CORPI CON MASSA ****************************
    G1_pp=z1/2*[-cos(q); -sin(q)]*q_p^2;
    G2_pp=z1*[-cos(q);-sin(q)]*q_p^2+z5*[-cos(phi2);-sin(phi2)]*x_p(2)^2+z5*[-sin(phi2);cos(phi2)]*x_pp(2);    
    % ****************************************************************************   
    
    % ANALISI DINAMICA ***********************************************************
    S=[1    0   1           0           0               0   0   0   0;...
       0    1   0           1           0               0   0   0   0;...
       0    0   -z1*sin(q)  z1*cos(q)   0               0   0   0   1;...
       0    0   -1          0           sin(phi2)       0   0   0   0;...
       0    0   0           -1          -cos(phi2)      0   0   0   0;...
       0    0   0           0           -z2             1   0   0   0;...
       0    0   0           0           -sin(phi2)      0   1   0   0;...
       0    0   0           0           cos(phi2)       0   0   1   0;...
       0    0   0           0           -z3*cos(chi)    -1  0   0   0];
    
   Q=[m1*G1_pp(1),m1*G1_pp(2),0,m2*G2_pp(1),m2*G2_pp(2),-m2*G2_pp(1)*z5*sin(phi2)+m2*G2_pp(2)*z2*cos(phi2)+I2*x_pp(2),0,0,0]';
   
   reazioni_e_M(:,count)=inv(S)*Q;
    % ****************************************************************************
    
end

% grafico della coppia del motore
figure(1)
hold on
plot(vet_q,reazioni_e_M(9,:))
xlabel('q [rad]')
ylabel('coppia dell attuatore [N*m]')