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

clc

% 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;

% Determinaçao grafica da variaçao do Xcm em funçao dos angulos q2L e q3L:

figure

syms q2L q3L q4L 
    
    r1 = L12/2;
    r2 = L23/2;
    r3 = L34/2;

    q4L = (((90)-(q2L+q3L))*pi/180);
    
    b11 = r1*cos(q2L*pi/180);
    b12 = L12*cos(q2L*pi/180) + r2*cos((q2L+q3L)*pi/180);
    b13 = L23*cos((q2L+q3L)*pi/180) + L12*cos(q2L*pi/180) + r3*cos((q2L+q3L+q4L)*pi/180);
    
    Xcm = (m1*b11 + m2*b12 + m3*b13)/(m1 + m2 + m3);
       
    ezsurf( Xcm, [60 90], [0 40])
   
    Xlabel('q2L')
    Ylabel('q3L')
    Zlabel('Xcm')
    Title('Grafico 3D, do M1 em funçao do q2L e do q3L. Para q4L sempre na vertical')
 %-----------------------------------------------------------------------------------------------------   
 %_____________________________________________________________________________________________________
 %_____________________________________________________________________________________________________
 %_____________________________________________________________________________________________________
 %_____________________________________________________________________________________________________
 

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

figure

syms q2L q3L q4L 
    
    r1 = L12/2;
    r2 = L23/2;
    r3 = L34/2;

    q4L = (((90)-(q2L+q3L))*pi/180);
    
    b11 = r1*cos(q2L*pi/180);
    b12 = L12*cos(q2L*pi/180) + r2*cos((q2L+q3L)*pi/180);
    b13 = L23*cos((q2L+q3L)*pi/180) + L12*cos(q2L*pi/180) + r3*cos((q2L+q3L+q4L)*pi/180);
    
    Xcm = (m1*b11 + m2*b12 + m3*b13)/(m1 + m2 + m3);
       
    q3L = 0;
    
    Xcmth1 = subs(Xcm);
    
    ezplot(Xcmth1 , [60 90])
    
    Ylabel('Xcm')
    Title('Xcm em funçao de q2L, para q3L=0º')
    grid on
    
% Variaçao do centro de massa em funçao de teta1 e com teta2 = 20º:

figure

syms q2L q3L q4L 
    
    r1 = L12/2;
    r2 = L23/2;
    r3 = L34/2;

    q4L = (((90)-(q2L+q3L))*pi/180);
    
    b11 = r1*cos(q2L*pi/180);
    b12 = L12*cos(q2L*pi/180) + r2*cos((q2L+q3L)*pi/180);
    b13 = L23*cos((q2L+q3L)*pi/180) + L12*cos(q2L*pi/180) + r3*cos((q2L+q3L+q4L)*pi/180);
    
    Xcm = (m1*b11 + m2*b12 + m3*b13)/(m1 + m2 + m3);
       
    q3L = 20;
    
    Xcmth1 = subs(Xcm);
    
    ezplot(Xcmth1 , [60 90])
    
    Ylabel('Xcm')
    Title('Xcm em funçao de q2L, para q3L=20º')
    grid on

% Variaçao do centro de massa em funçao de q2L e com q3L = 40º:

figure

syms q2L q3L q4L 
    
    r1 = L12/2;
    r2 = L23/2;
    r3 = L34/2;

    q4L = (((90)-(q2L+q3L))*pi/180);
    
    b11 = r1*cos(q2L*pi/180);
    b12 = L12*cos(q2L*pi/180) + r2*cos((q2L+q3L)*pi/180);
    b13 = L23*cos((q2L+q3L)*pi/180) + L12*cos(q2L*pi/180) + r3*cos((q2L+q3L+q4L)*pi/180);
    
    Xcm = (m1*b11 + m2*b12 + m3*b13)/(m1 + m2 + m3);
       
    q3L = 40;
    
    Xcmth1 = subs(Xcm);
    
    ezplot(Xcmth1 , [60 90])
    
    Ylabel('Xcm')
    Title('Xcm em funçao de q2L, para q3L=40º')
    grid on
    
%___________________________________________________________________________________________________________
%___________________________________________________________________________________________________________
%___________________________________________________________________________________________________________
%___________________________________________________________________________________________________________
%___________________________________________________________________________________________________________
    
% Variaçao do centro de massa em funçao de q3L e com q2L = 60º:

figure

syms q2L q3L q4L 
    
    r1 = L12/2;
    r2 = L23/2;
    r3 = L34/2;

    q4L = (((90)-(q2L+q3L))*pi/180);
    
    b11 = r1*cos(q2L*pi/180);
    b12 = L12*cos(q2L*pi/180) + r2*cos((q2L+q3L)*pi/180);
    b13 = L23*cos((q2L+q3L)*pi/180) + L12*cos(q2L*pi/180) + r3*cos((q2L+q3L+q4L)*pi/180); 
    
    Xcm = (m1*b11 + m2*b12 + m3*b13)/(m1 + m2 + m3);
       
    q2L = 60;
    
    Xcmth2 = subs(Xcm);
    
    ezplot(Xcmth2 , [0 40])
    
    Ylabel('Xcm')
    Title('Xcm em funçao de q3L, para q2L=60º')
    grid on
    
% Variaçao do centro de massa em funçao de q3L e com q2L = 75º:

figure

syms q2L q3L q4L 
    
    r1 = L12/2;
    r2 = L23/2;
    r3 = L34/2;

    q4L = (((90)-(q2L+q3L))*pi/180);
    
    b11 = r1*cos(q2L*pi/180);
    b12 = L12*cos(q2L*pi/180) + r2*cos((q2L+q3L)*pi/180);
    b13 = L23*cos((q2L+q3L)*pi/180) + L12*cos(q2L*pi/180) + r3*cos((q2L+q3L+q4L)*pi/180);
    
    Xcm = (m1*b11 + m2*b12 + m3*b13)/(m1 + m2 + m3);
       
    q2L = 75;
    
    Xcmth2 = subs(Xcm);
    
    ezplot(Xcmth2 , [0 40])
    
    Ylabel('Xcm')
    Title('Xcm em funçao de q3L, para q2L=75º')
    grid on
    
% Variaçao do centro de massa em funçao de q3L e com q2L = 90º:

figure

syms q2L q3L q4L 
    
    r1 = L12/2;
    r2 = L23/2;
    r3 = L34/2;

    q4L = (((90)-(q2L+q3L))*pi/180);
    
    b11 = r1*cos(q2L*pi/180);
    b12 = L12*cos(q2L*pi/180) + r2*cos((q2L+q3L)*pi/180);
    b13 = L23*cos((q2L+q3L)*pi/180) + L12*cos(q2L*pi/180) + r3*cos((q2L+q3L+q4L)*pi/180);
    
    Xcm = (m1*b11 + m2*b12 + m3*b13)/(m1 + m2 + m3);
       
    q2L = 90;
    
    Xcmth2 = subs(Xcm);
    
    ezplot(Xcmth2 , [0 40])
    
    Ylabel('Xcm')
    Title('Xcm em funçao de q3L, para q2L=90º')
    grid on
    
    