% ----------------------------------------------------------
% Corso di Meccanica applicata alle macchine 
% ----------------------------------------------------------
% Esercitazione n. 15
% ----------------------------------------------------------
% Questo programma risolve, con il metodo del partizionamento 
% delle coordinte, il problema dinamico diretto di un triplo pendolo.
% Tale versione è molto elementare e si rilascia liberamente
% per scopi esclusivamente didattici. La versione è molto semplificata
% per aumentarne la leggibilità e la comprensione per chi si 
% dedica per la prima volta alla Dinamica dei sistemi multibody
% -----------------------------------------------------------------
% Per maggiori informazioni
% consultare il sito principale di KynSinth
% http://dma.ing.uniroma1.it/users/lsm_progfunz/index.html
% 
% --------------------------------------------------------------------
% This Project has been developed since september 2010 
% as an Open Source program at Sapienza,
% University of Rome, by prof. N.P. Belfiore, 
% as an activity of the Course of "Functional Design". 
% Prof. E. Pennestrì joined the program  
% at Tor Vergata University since november 2010.
%
% ---------------------------------------
% Common creative licence published and released at
% http://dma.ing.uniroma1.it/users/lsm_progfunz/index.html
% as KinSynth
%---------------------------------------------------------------
% You may use it and suggest any modification by sending any new versions to 
% belfiore@dma.ing.uniroma1.it . 
% In this case the new code will be released by including your name 
% within its credits. The Author and his contributors can not be 
% considered in any form as responsible for any misuse of the program 
% or for any damage due to its use. 
% ---------------------------------------------------------------------
% Triple Pendulum Forward Dynamic, 
% Release 1.0
% Credits: ...
% ----------------------------------------------------------------------
% ----------------------------------------------------------------------
%
% This is the beginning
%
%
clear all
close all
%
printf('Hello, this is the beginning ! \n');
%
% Dati del problema
%
% telaio =1, le altre aste sono la 2, 3 e 4
%
F = 3;
n = 9;
p = n - F;
g = 9.8 % m/s2
% lunghezze
l2 = 1;
l3 = 0.7;
l4 = 1;
% masse
m2 = 10;
m3 = 7;
m4 = 10;
% momenti d'inerzia baricentrici
% Si ipotizzano le seguenti relazioni dovute
% alla scelta arbitraria della forma dei 3 corpi
IG2 = (1/12)*m2*l2^2;
IG3 =  (1/12)*m3*l3^2; 
IG4 = (1/12)*m4*l4^2;
%
% Se uso Matlab
% attesa = 0.01;
%
% CONDIZIONI INIZIALI
%
% Variabili indipendenti
%
theta2 = (-5)*(pi/180);
theta3 = (-15)*(pi/180);
theta4 = (-25)*(pi/180);
%
omega2 = 0;
omega3 = 0;
omega4 = 0;
%
% Assegnazione dell'intervallo di discretizzazione
% In questo caso è costante (molto elementare)
% e del tempo di discretizzazione
%
DT = 0.01;
T = 3;
kmax = fix(T/DT);
%
T2 = zeros(1,kmax);
T3 = zeros(1,kmax);
T4 = zeros(1,kmax);
O2 = zeros(1,kmax);
O3 = zeros(1,kmax);
O4 = zeros(1,kmax);
%
% Assegnazione iniziale
%
T2(1) = theta2;
T3(1) = theta3;
T4(1) = theta4;
O2(1) = omega2;
O3(1) = omega3;
O4(1) = omega4;
%
k  = 1;
%
%
% Matrice delle masse (fuori dal ciclo in questo caso elementare)
%
Muu = diag([m2,m2,m3,m3,m4,m4]);
Mvv=diag([IG2,IG3,IG4]);
Mvu = zeros(3,6);
Muv=zeros(6,3);
%
% Componente lagrangiana della sollecitaizone (con 
% partizionamento) (in questo caso elementare è fuori dal ciclo)
%
Qv =transpose([0,0,0]);
Qu =transpose( [0, (-m2 * g), 0, (-m3*g), 0, (-m4*g)]);
%
while ( k < (kmax-1) )
% 
%  INIZIO DEL CICLO DI CALCOLO ITERATIVO
%
theta2=T2(k) ;
theta3=T3(k);
theta4=T4(k) ;
omega2=O2(k) ;
omega3=O3(k) ;
omega4=O4(k);
%
% Calcolo dello jacobiano
%
Psiq = [1 0 0 0 0 0 l2 * sin(theta2) / 0.2e1 0 0; 0 1 0 0 0 0 -l2 * cos(theta2) / 0.2e1 0 0; -1 0 1 0 0 0 l2 * sin(theta2) / 0.2e1 l3 * sin(theta3) / 0.2e1 0; 0 -1 0 1 0 0 -l2 * cos(theta2) / 0.2e1 -l3 * cos(theta3) / 0.2e1 0; 0 0 -1 0 1 0 0 l3 * sin(theta3) / 0.2e1 l4 * sin(theta4) / 0.2e1; 0 0 0 -1 0 1 0 -l3 * cos(theta3) / 0.2e1 -l4 * cos(theta4) / 0.2e1;];
%
% Estrazione dei minori Psiu e Psiv
%
Psiu = Psiq(1:p,1:p);
Psiv = Psiq(1:p,(p+1):n);
%
% Calcolo del vettore gamma, che chiameremo gama
%
gamaT = [-l2 * cos(theta2) * omega2 ^ 2 / 0.2e1 -l2 * sin(theta2) * omega2 ^ 2 / 0.2e1 -l2 * cos(theta2) * omega2 ^ 2 / 0.2e1 - l3 * cos(theta3) * omega3 ^ 2 / 0.2e1 -l2 * sin(theta2) * omega2 ^ 2 / 0.2e1 - l3 * sin(theta3) * omega3 ^ 2 / 0.2e1 -l3 * cos(theta3) * omega3 ^ 2 / 0.2e1 - l4 * cos(theta4) * omega4 ^ 2 / 0.2e1 -l3 * sin(theta3) * omega3 ^ 2 / 0.2e1 - l4 * sin(theta4) * omega4 ^ 2 / 0.2e1];
gama = transpose(gamaT);
%
% Calcolo delle accelerazioni
%
invPsiu = inv(Psiu);
Mh = Mvv-Mvu*invPsiu*Psiv-transpose(Psiv)*transpose(invPsiu)*(Muv-Muu*invPsiu*Psiv);
Qh=Qv-Mvu*invPsiu*gama-transpose(Psiv)*transpose(invPsiu)*(Qu-Muu*invPsiu*gama);
%
ddv=Mh\Qh;
ddu=invPsiu*(gama-Psiv*ddv);
ddq(1:p)=ddu;
ddq((p+1):n)=ddv;
ddq=transpose(ddq);
%
%  Fine calcolo delle accelerazioni
%
%  Calcolo approssimato delle velocità e delle posizioni
%
O2(k+1) = O2(k) + ddv(1)*DT;
O3(k+1) = O3(k) + ddv(2)*DT;
O4(k+1) = O4(k) + ddv(3)*DT;
T2(k+1) = T2(k) + O2(k)*DT;
T3(k+1) = T3(k) + O3(k)*DT;
T4(k+1) = T4(k) + O4(k)*DT;
%
%
if (k/10) == fix(k/10)  
printf(' Progressione del calcolo \n'); k
endif;
k = k +1;
endwhile;
%
%
% CICLO PER IL DISEGNO
%
% 
k = 1
%
theta2 = T2(k);
theta3 = T3(k);
theta4 = T4(k);
%% 
x2 = l2 * cos(theta2) / 0.2e1;
y2 =l2 * sin(theta2) / 0.2e1;
x3 = x2 +l2 * cos(theta2) / 0.2e1 +l3 * cos(theta3) / 0.2e1;
y3 = y2 + l2 * sin(theta2) / 0.2e1 + l3 * sin(theta3) / 0.2e1;
x4 = x3 + l3 * cos(theta3) / 0.2e1 + l4 * cos(theta4) / 0.2e1;
y4 = y3 + l3 * sin(theta3) / 0.2e1 + l4 * sin(theta4) / 0.2e1;
%
AX = x2 - (l2/2)*cos(theta2);
AY = y2 - (l2/2)*sin(theta2);
BX = x2 + (l2/2)*cos(theta2);
BY = y2 + (l2/2)*sin(theta2);
% disegno la seconda asta
CX = x3 - (l3/2)*cos(theta3);
CY = y3 - (l3/2)*sin(theta3);
DX = x3 + (l3/2)*cos(theta3);
DY = y3 + (l3/2)*sin(theta3);
% disegno la terza asta
EX = x4 - (l4/2)*cos(theta4);
EY = y4 - (l4/2)*sin(theta4);
FX = x4  + (l4/2)*cos(theta4);
FY = y4  + (l4/2)*sin(theta4);
Xcatena = [AX,BX,EX,FX];
Ycatena = [AY,BY,EY,FY];
%
%
%
% h3 = plot(Xcentri,Ycentri,'-oy','LineWidth',2);
h3 = plot(Xcatena,Ycatena,'-og','LineWidth',2);
axis([-3 3 -3 3],'equal');
grid on;
ylabel(' Asse y del piano');
xlabel(' Asse x del piano');  
title([' Animazione grafica per il moto delle tre aste ']);
set(h3,'EraseMode','xor')
%
%
drawnow
%
k = 2;
%
while (k<(kmax-1))
%  pause(attesa); % se uso MatLab
% 
%  INIZIO DEL CICLO DI CALCOLO ITERATIVO
%
% Numero di frames che salto nell'animazione
% se aumeto Nframe allora aumento la velocità ma perdo accuratezza
%
Dframe = 2;
%
if (k/Dframe) == fix(k/Dframe)    % se uso Octave
% printf(' Progressione del calcolo (secondo ciclo) \n'); k
%
drawnow
%
theta2 = T2(k);
theta3 = T3(k);
theta4 = T4(k);
%% 
x2 = l2 * cos(theta2) / 0.2e1;
y2 =l2 * sin(theta2) / 0.2e1;
x3 = x2 +l2 * cos(theta2) / 0.2e1 +l3 * cos(theta3) / 0.2e1;
y3 = y2 + l2 * sin(theta2) / 0.2e1 + l3 * sin(theta3) / 0.2e1;
x4 = x3 + l3 * cos(theta3) / 0.2e1 + l4 * cos(theta4) / 0.2e1;
y4 = y3 + l3 * sin(theta3) / 0.2e1 + l4 * sin(theta4) / 0.2e1;
%
AX = x2 - (l2/2)*cos(theta2);
AY = y2 - (l2/2)*sin(theta2);
BX = x2 + (l2/2)*cos(theta2);
BY = y2 + (l2/2)*sin(theta2);
% disegno la seconda asta
CX = x3 - (l3/2)*cos(theta3);
CY = y3 - (l3/2)*sin(theta3);
DX = x3 + (l3/2)*cos(theta3);
DY = y3 + (l3/2)*sin(theta3);
% disegno la terza asta
EX = x4 - (l4/2)*cos(theta4);
EY = y4 - (l4/2)*sin(theta4);
FX = x4  + (l4/2)*cos(theta4);
FY = y4  + (l4/2)*sin(theta4);
Xcatena = [AX,BX,EX,FX];
Ycatena = [AY,BY,EY,FY];
%
set(h3,'XData',Xcatena,'YData',Ycatena)
%
endif;
k = k +1;
endwhile;
%
%
printf('I have done! \n');
printf('Good bye class ! \n');