% MOMENTO 2 EM FUNÇAO DE TETA1, E DEPOIS EM FUNÇAO DE TETA2. GRAFICOS 2D
% Com restriçao do centro de massa para a variaçao de teta1 e teta2

% Altura do robot sera de 80cm nesta analise.


% Pesos dos elos:

P1 = 1.5*9.81;

P2 = 1.5*9.81;

P3 = 11.0*9.81;


% Comprimentos dos elos:

L12 = 0.175;

L23 = 0.175;

L34 = 0.4;


%---------------------------------------------------------------------------------------------
%---------------------------------------------------------------------------------------------
%---------------------------------------------------------------------------------------------
%---------------------------------------------------------------------------------------------


% Grafico do Momento exercido sobre a junta do joelho, em funçao de q2L
% para um q3L minimo de 0º, e com q4L fixo em 90º.

figure

syms q2L q3L q4L 

    r1 = L12/2;
    r2 = L23/2;
    r3 = L34/2;

    q4L = (((90)-(q2L+q3L))*pi/180);
    
    b22 = r2*cos((q2L+q3L)*pi/180);
    b23 = L23*cos((q2L+q3L)*pi/180)+r3*cos((q2L+q3L+q4L)*pi/180);
    
    t3L = P2*b22+P3*b23;
    
    q3L = 0
   
    t3L_q2L = subs(t3L)
        
    ezplot(t3L_q2L, [71.26 90]) 
    
    grid on
    Ylabel('t3L')
    Title('t3L em funçao de q2L para q3L=0º')

% Grafico do Momento exercido sobre a junta do joelho, em funçao de q2L
% para um q3L medio de 20º, e com q4L fixo em 90º.

figure

syms q2L q3L q4L 

    r1 = L12/2;
    r2 = L23/2;
    r3 = L34/2;

    q4L = (((90)-(q2L+q3L))*pi/180);
    
    b22 = r2*cos((q2L+q3L)*pi/180);
    b23 = L23*cos((q2L+q3L)*pi/180)+r3*cos((q2L+q3L+q4L)*pi/180);
    
    t3L = P2*b22+P3*b23;
    
    q3L = 20
   
    t3L_q2L = subs(t3L)
        
    ezplot(t3L_q2L, [60 87.35]) 
    
    grid on
    Ylabel('t3L')
    Title('t3L em funçao de q2L para q3L=20º')
    
% Grafico do Momento exercido sobre a junta do joelho, em funçao de q2L
% para um q3L maximo de 40º, e com q4L fixo em 90º.

figure

syms q2L q3L q4L 

    r1 = L12/2;
    r2 = L23/2;
    r3 = L34/2;

    q4L = (((90)-(q2L+q3L))*pi/180);
    
    b22 = r2*cos((q2L+q3L)*pi/180);
    b23 = L23*cos((q2L+q3L)*pi/180)+r3*cos((q2L+q3L+q4L)*pi/180);
    
    t3L = P2*b22+P3*b23;
    
    q3L = 40
   
    t3L_q2L = subs(t3L)
        
    ezplot(t3L_q2L, [60 74]) 
    
    grid on
    Ylabel('t3L')
    Title('t3L em funçao de q2L para q3L=40º')

    
%_____________________________________________________________________________________________
%_____________________________________________________________________________________________    
%_____________________________________________________________________________________________
%_____________________________________________________________________________________________



% Grafico do Momento exercido sobre a junta do joelho, em funçao de q3L
% para um q2L minimo de 60º, e com q4L fixo em 90º.

figure

syms q2L q3L q4L 

    r1 = L12/2;
    r2 = L23/2;
    r3 = L34/2;

    q4L = (((90)-(q2L+q3L))*pi/180);
    
    b22 = r2*cos((q2L+q3L)*pi/180);
    b23 = L23*cos((q2L+q3L)*pi/180)+r3*cos((q2L+q3L+q4L)*pi/180);
    
    t3L = P2*b22+P3*b23;
    
    q2L = 60;
    
    t3L_q3L = subs(t3L);
        
    ezplot(t3L_q3L, [17 40]) 
    
    grid on
    Ylabel('t3L')
    Title('t3L em funçao de q3L para q2L=60º')
    

% Grafico do Momento exercido sobre a junta do joelho, em funçao de q3L
% para um q2L medio de 75º, e com q4L fixo em 90º.

figure

syms q2L q3L q4L 

    r1 = L12/2;
    r2 = L23/2;
    r3 = L34/2;

    q4L = (((90)-(q2L+q3L))*pi/180);
    
    b22 = r2*cos((q2L+q3L)*pi/180);
    b23 = L23*cos((q2L+q3L)*pi/180)+r3*cos((q2L+q3L+q4L)*pi/180);
    
    t3L = P2*b22+P3*b23;
    
    q2L = 75;
    
    t3L_q3L = subs(t3L);
        
    ezplot(t3L_q3L, [0 39]) 
    
    grid on
    Ylabel('t3L')
    Title('t3L em funçao de q3L para q2L=75º')
    
    
% Grafico do Momento exercido sobre a junta do joelho, em funçao de q3L
% para um q2L maximo de 90º, e com q4L fixo em 90º.

figure

syms q2L q3L q4L 

    r1 = L12/2;
    r2 = L23/2;
    r3 = L34/2;

    q4L = (((90)-(q2L+q3L))*pi/180);
    
    b22 = r2*cos((q2L+q3L)*pi/180);
    b23 = L23*cos((q2L+q3L)*pi/180)+r3*cos((q2L+q3L+q4L)*pi/180);
    
    t3L = P2*b22+P3*b23;
    
    q2L = 90;
    
    t3L_q3L = subs(t3L);
        
    ezplot(t3L_q3L, [0 15.5]) 
    
    grid on
    Ylabel('t3L')
    Title('t3L em funçao de q3L para q2L=90º')
    

    
    clc