% Altura do robot sera de 80cm nesta analise.

% Pesos dos elos:

P1 = 0.75*9.81;

P2 = 0.75*9.81;

P3 = 3*9.81;

% Comprimentos dos elos:

L12 = 0.16;

L23 = 0.16;

L34 = 0.4;

L44 = 0.15;

L5CM = 0.075;



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

    
% Grafico do Momento exercido sobre a junta do tornozelo,
% em funçao de 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);
    
    t1L = P1*(b41+L44) + P2*(b42+L44) + P3*(b43+L5CM)
  
    ezsurf(t1L,[70 120],[-30 30] )

    grid on
    
    Title('Grafico 3D de t1L em funçao de q1L e q5L')
    Xlabel('q1L')
    Ylabel('q5L')
    Zlabel('t1L')
    

% Grafico do Momento exercido sobre a junta da anca,
% em funçao de q1L, e q5L.

figure

syms q1L q5L 

    r1 = L12/2;
    r2 = L23/2;
    r3 = L34/2;
    
    b53 = r3*cos((q1L+q5L)*pi/180);
    
    t5L = P3*(b53+L5CM);
     
    ezsurf(t5L, [70 120], [-30 30])
      
    Title('Grafico 3D de t5L em funçao de q1L e q5L')
    Xlabel('q1L')
    Ylabel('q5L')
    Zlabel('t5L')  
    
    
    
%---------------------------------------------------------------------------------------------
%---------------------------------------------------------------------------------------------
%---------------------------------------------------------------------------------------------
%---------------------------------------------------------------------------------------------
%---------------------------------------------------------------------------------------------
%---------------------------------------------------------------------------------------------


% Grafico do Momento exercido sobre a junta do tornozelo,
% em funçao de q1L, e com q5L fixo em 90º.

figure

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

    q5L = (((90)-(q1L))*pi/180);
    
    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);
    
    t1L = P1*(b41+L44) + P2*(b42+L44) + P3*(b43+L5CM)
  
    ezplot(t1L, [75 105])

    grid on
    
    Title('Grafico 2D de t1L em funçao de q1L')
    Xlabel('q1L')
    Ylabel('t1L')
  
% Grafico do Momento exercido sobre a junta da anca,
% em funçao de q1L, e com q5L fixo em 90º.

figure

syms q1L q5L 

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

    q5L = (((90)-(q1L))*pi/180);
    
    b53 = r3*cos((q1L+q5L)*pi/180);
    
    t5L = P3*(b53+L5CM);
     
    ezplot(t5L, [-15 15])
    
    grid on
    
    Title('Grafico 2D de t5L em funçao de q1L')
    Xlabel('q1L')
    Ylabel('t5L')
    
    
   clc
   