% MOMENTO 1 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 = 0*9.81;


% Comprimentos dos elos:

L12 = 0.175;

L23 = 0.175;

L34 = 0.4;

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


% Grafico do Momento exercido sobre a junta do tornozelo, 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);
    
    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);
       
    t4L = P1*b12 + P2*b13;
      
    q2L = 90;
  
    t4L_q2L = subs(t2L)
    
    ezplot(t4L_q2L , [0 90])
    
    grid on
    Ylabel('t4L')
    Title('t4L em funçao de q2L para q3L=0º')
    

% Grafico do Momento exercido sobre a junta do tornozelo, 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);
    
    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);
       
    t3L = P1*b12;
      
    q2L = 90;
  
    t3L_q2L = subs(t2L)
    
    ezplot(t3L_q2L , [0 90])
    
    grid on
    Ylabel('t3L')
    Title('t3L em funçao de q2L para q3L=0º') 