% analisi di velocità del quadrilatero articolato con coppia prismatica
% rispetto ad "analisi_cinematica_diadi" viene aggiunto un blocco per analisi di velocità
% ipotizziamo che la manovella ruoti a velocità costante

clear 
clf

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

% dati geometrici
z1=1;
z4=15;
z3=4;
chi=40*pi/180;     % valore dato in gradi

% 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);
    % ****************************************************************************
    
end

% grafico delle velocità
figure(1)
hold on
plot(vet_q,vet_z2_p)
xlabel('q [rad]')
ylabel('velocità di z2 [m/s]')

figure(2)
hold on
plot(vet_q,vet_phi2_p)
xlabel('q [rad]')
ylabel('velocità di phi2 [rad/s]')