% CALCULO DA VARIAÇAO DO CENTRO DE MASSA TOTAL DO SISTEMA.


% Massas dos elos (em Kg):

m1 = 1.5;

m2 = 1.5;

m3 = 8;

% Comprimentos dos elos (em m):

L12 = 0.175;

L23 = 0.175;

L34 = 0.4;

L44 = 0.15;

L5CM = 0.125;


% Determinaçao grafica da variaçao do Xcm em funçao dos angulos q1L e q5L:

figure

syms q1L q5L 
    
    r1 = L12/2;
    r2 = L23/2;
    r3 = L34/2;
   
    b41 = r1*cos(q1L*pi/180);
    b42 = (L12+r2)*cos(q1L*pi/180);
    b43 = (L12+L23)*cos((q1L)*pi/180) + r3*cos((q1L+q5L)*pi/180);
    
    Xcm2 = (m1*b41 + m1*L44 + m2*b42 + m2*L44 + m3*b43 + m3*L5CM)/(2*m1 + 2*m2 + 2*m3);
       
    ezsurf( Xcm2,[75 105],[-15 15])
   
    Xlabel('q1L')
    Ylabel('q5L')
    Zlabel('Xcm2')
    Title('Grafico 3D, do Xcm2 em funçao do q1L e do q5L')
    
    
 %-----------------------------------------------------------------------------------------------------   
 %_____________________________________________________________________________________________________
 %_____________________________________________________________________________________________________
 %_____________________________________________________________________________________________________
 %_____________________________________________________________________________________________________
 

% Variaçao do centro de massa em funçao de teta1 e com teta2 = 0º:

figure

syms q1L q5L     
    r1 = L12/2;
    r2 = L23/2;
    r3 = L34/2;
  
    b41 = r1*cos(q1L*pi/180);
    b42 = (L12+r2)*cos(q1L*pi/180);
    b43 = (L12+L23)*cos((q1L)*pi/180) + r3*cos((q1L+q5L)*pi/180);
    
    Xcm2 = (m1*b41 + m1*L44 + m2*b42 + m2*L44 + m3*b43 + m3*L5CM)/(2*m1 + 2*m2 + 2*m3);
   
    q5L = -30;
    
    Xcmth4 = subs(Xcm2);
    
    ezplot(Xcmth4 , [70 120])
    
    Ylabel('Xcm')
    Title('Xcm em funçao de q1L, para q5L=-30')
    grid on
    
    
% Variaçao do centro de massa em funçao de teta1 e com teta2 = 45º:

figure

syms q1L q5L 

    r1 = L12/2;
    r2 = L23/2;
    r3 = L34/2;
    
    b41 = r1*cos(q1L*pi/180);
    b42 = (L12+r2)*cos(q1L*pi/180);
    b43 = (L12+L23)*cos((q1L)*pi/180) + r3*cos((q1L+q5L)*pi/180);
    
    Xcm2 = (m1*b41 + m1*L44 + m2*b42 + m2*L44 + m3*b43 + m3*L5CM)/(2*m1 + 2*m2 + 2*m3);
       
    q5L = 0;
    
    Xcmth4 = subs(Xcm2);
    
    ezplot(Xcmth4 , [70 120])
    
    Ylabel('Xcm')
    Title('Xcm em funçao de q1L, para q5L=0')
    grid on

    
% Variaçao do centro de massa em funçao de teta1 e com teta2 = 90º:

figure

syms q1L q5L
    
    r1 = L12/2;
    r2 = L23/2;
    r3 = L34/2;

    b41 = r1*cos(q1L*pi/180);
    b42 = (L12+r2)*cos(q1L*pi/180);
    b43 = (L12+L23)*cos((q1L)*pi/180) + r3*cos((q1L+q5L)*pi/180);
    
    Xcm2 = (m1*b41 + m1*L44 + m2*b42 + m2*L44 + m3*b43 + m3*L5CM)/(2*m1 + 2*m2 + 2*m3);
       
    q5L = 30;
    
    Xcmth4 = subs(Xcm2);
    
    ezplot(Xcmth4 , [70 120])
    
    Ylabel('Xcm')
    Title('Xcm em funçao de q1L, para q5L=30')
    grid on

    
%___________________________________________________________________________________________________________
%___________________________________________________________________________________________________________
%___________________________________________________________________________________________________________
%___________________________________________________________________________________________________________
%___________________________________________________________________________________________________________
    
% Variaçao do centro de massa em funçao de teta2 e com teta1 = 45º:

figure

syms q1L q5L 
    
    r1 = L12/2;
    r2 = L23/2;
    r3 = L34/2;

    b41 = r1*cos(q1L*pi/180);
    b42 = (L12+r2)*cos(q1L*pi/180);
    b43 = (L12+L23)*cos((q1L)*pi/180) + r3*cos((q1L+q5L)*pi/180);
    
    Xcm2 = (m1*b41 + m1*L44 + m2*b42 + m2*L44 + m3*b43 + m3*L5CM)/(2*m1 + 2*m2 + 2*m3);
       
    q1L = 70;
    
    Xcmth5 = subs(Xcm2);
    
    ezplot(Xcmth5 , [-30 30])
    
    Ylabel('Xcm')
    Title('Xcm em funçao de q5L, para q1L=70')
    grid on

    
% Variaçao do centro de massa em funçao de teta2 e com teta1 = 90º:

figure

syms q1L q5L 
    
    r1 = L12/2;
    r2 = L23/2;
    r3 = L34/2;

    b41 = r1*cos(q1L*pi/180);
    b42 = (L12+r2)*cos(q1L*pi/180);
    b43 = (L12+L23)*cos((q1L)*pi/180) + r3*cos((q1L+q5L)*pi/180);
    
    Xcm2 = (m1*b41 + m1*L44 + m2*b42 + m2*L44 + m3*b43 + m3*L5CM)/(2*m1 + 2*m2 + 2*m3);
       
    q1L = 90;
    
    Xcmth5 = subs(Xcm2);
    
    ezplot(Xcmth5 , [-30 30])
    
    Ylabel('Xcm')
    Title('Xcm em funçao de q5L, para q1L=90')
    grid on

    
% Variaçao do centro de massa em funçao de teta2 e com teta1 = 110º:

figure

syms q1L q5L 
    
    r1 = L12/2;
    r2 = L23/2;
    r3 = L34/2;

    b41 = r1*cos(q1L*pi/180);
    b42 = (L12+r2)*cos(q1L*pi/180);
    b43 = (L12+L23)*cos((q1L)*pi/180) + r3*cos((q1L+q5L)*pi/180);
    
    Xcm2 = (m1*b41 + m1*L44 + m2*b42 + m2*L44 + m3*b43 + m3*L5CM)/(2*m1 + 2*m2 + 2*m3);
       
    q1L = 120;
    
    Xcmth5 = subs(Xcm2);
    
    ezplot(Xcmth5 , [-30 30])
    
    Ylabel('Xcm')
    Title('Xcm em funçao de q5L, para q1L=120')
    grid on

    
    