%----------------------------------------------------------------------
%                             Projecto final
% 
%                    Luís Alberto Soares Rêgo nº. 20008 
%              Renato Francisco dos Santos Barbosa n.º 18382
%                         Engenharia Mecânica
%                       
%                 Cinematica directa das pernas do robot
%----------------------------------------------------------------------
%
% Função ROBPLOT - Desenho gráfico e animaçao do robô a partir da descrição cinemática DH 
%                  e das trajectórias das juntas q; desenha o sistema de coordenadas 
%                  associado ao elemento terminal para mostrar a posiç~ao final das pernas 



function [CMJuntas,CMTotal,tL,tR,mov] = ROBPLOT(Duas_Pernas,Perna_Tronco,Q,Qf)


% ANIMAÇAO, DO ROBOT ENTRE OS PONTOS INICIAL E FINAl:

NN = 40;

M1L = 0;
M2L = 0;
M3L = 0;
M4L = 0;
M5L = 0;
M5R = 0;
M4R = 0;
M3R = 0;
M2R = 0;
M1R = 0;

Q = Q*pi/180;
Qf = Qf*pi/180;

for j = 1:NN
    
    Q(1) = Q(1) + j*((Qf(1)-Q(1))/NN);
    Q(2) = Q(2) + j*((Qf(2)-Q(2))/NN); 
    Q(3) = Q(3) + j*((Qf(3)-Q(3))/NN);
    Q(4) = Q(4) + j*((Qf(4)-Q(4))/NN);
    Q(5) = Q(5) + j*((Qf(5)-Q(5))/NN);
    Q(6) = Q(6) + j*((Qf(6)-Q(6))/NN);
    Q(7) = Q(7) + j*((Qf(7)-Q(7))/NN);
    Q(8) = Q(8) + j*((Qf(8)-Q(8))/NN);
    Q(9) = Q(9) + j*((Qf(9)-Q(9))/NN);
    Q(10) = Q(10) + j*((Qf(10)-Q(10))/NN);
    
    Q = [Q(1) Q(2) Q(3) Q(4) Q(5) Q(6) Q(7) Q(8) Q(9) Q(10)];
     [RTG1,Pf1] = CINDIR(Perna_Tronco,Q);  % Chama-se a funçao CINDIR que permite o calculo dos pontos a desenhar, 
                                          % a partir da transformaçao efectuada, isto par
    [RTG,Pf] = CINDIR(Duas_Pernas,Q); % Chama-se a funçao CINDIR que permite o calculo dos pontos a desenhar, 
                                      % a partir da transformaçao efectuada.
    
%    a o elo do tronco.                             
                                
% DETERMINAÇAO DOS BINARIOS E DOS CENTROS DE MASSA PARA AS DIFERENTES JUNTAS:
% Dados:
m0 = 0.28;
m1 = 0.20;
m2 = 0.63;
m3 = 0.26;
m4 = 0.21;
m5 = 5.5;
g = 9.81;
Lo = [0;0;10];

% Determinaçao dos centros de massa dos elos:
CM = [(((Pf(1,2:11)-Pf(1,1:10))/2)+Pf(1,1:10));(((Pf(2,2:11)-Pf(2,1:10))/2)+Pf(2,1:10));(((Pf(3,2:11)-Pf(3,1:10))/2)+Pf(3,1:10))];
CM3 = (Pf1(:,7));
CMTotal = [Lo(:,1) CM(:,1) CM(:,2) CM(:,3) CM(:,4) CM3(:,1) CM(:,6) CM(:,7) CM(:,8) CM(:,9) CM(:,10)];
% plot3(CMTotal(1,:),CMTotal(2,:),CMTotal(3,:),'m*')
% hold on

% Determinaçao das coordenadas centro de massa total
CMJunta1 = (((m0*(CMTotal(:,11)))+(m1*(CMTotal(:,2)+CMTotal(:,10)))+(m2*(CMTotal(:,3)+CMTotal(:,9)))+(m3*(CMTotal(:,4)+CMTotal(:,8)))+(m4*(CMTotal(:,5)+CMTotal(:,7)))+(m5*(CMTotal(:,6))))/(2*m0+2*m1+2*m2+2*m3+2*m4+m5));
CMJunta2 = (((m0*(CMTotal(:,11)))+(m1*(CMTotal(:,10)))+(m2*(CMTotal(:,3)+CMTotal(:,9)))+(m3*(CMTotal(:,4)+CMTotal(:,8)))+(m4*(CMTotal(:,5)+CMTotal(:,7)))+(m5*(CMTotal(:,6))))/(m0+m1+2*m2+2*m3+2*m4+m5));
CMJunta3 = (((m0*(CMTotal(:,11)))+(m1*(CMTotal(:,10)))+(m2*(CMTotal(:,9)))+(m3*(CMTotal(:,4)+CMTotal(:,8)))+(m4*(CMTotal(:,5)+CMTotal(:,7)))+(m5*(CMTotal(:,6))))/(m0+m1+m2+2*m3+2*m4+m5));
CMJunta4 = (((m0*(CMTotal(:,11)))+(m1*(CMTotal(:,10)))+(m2*(CMTotal(:,9)))+(m3*(CMTotal(:,8)))+(m4*(CMTotal(:,5)+CMTotal(:,7)))+(m5*(CMTotal(:,6))))/(m0+m1+m2+m3+2*m4+m5));
CMJunta5 = (((m0*(CMTotal(:,11)))+(m1*(CMTotal(:,10)))+(m2*(CMTotal(:,9)))+(m3*(CMTotal(:,8)))+(m4*(CMTotal(:,7)))+(m5*(CMTotal(:,6))))/(m0+m1+m2+m3+m4+m5));
CMJunta6 = (((m0*(CMTotal(:,11)))+(m1*(CMTotal(:,10)))+(m2*(CMTotal(:,9)))+(m3*(CMTotal(:,8)))+(m4*(CMTotal(:,7))))/(m0+m1+m2+m3+m4));
CMJunta7 = (((m0*(CMTotal(:,11)))+(m1*(CMTotal(:,10)))+(m2*(CMTotal(:,9)))+(m3*(CMTotal(:,8))))/(m0+m1+m2+m3));
CMJunta8 = (((m0*(CMTotal(:,11)))+(m1*(CMTotal(:,10)))+(m2*(CMTotal(:,9))))/(m0+m1+m2));
CMJunta9 = (((m0*(CMTotal(:,11)))+(m1*(CMTotal(:,10))))/(m0+m1));
CMJunta10 = ((m0*(CMTotal(:,11)))/(m0));

CMJuntas = [CMJunta1 CMJunta2 CMJunta3 CMJunta4 CMJunta5 CMJunta6 CMJunta7 CMJunta8 CMJunta9 CMJunta10];
% plot3(CMJuntas(1,:),CMJuntas(2,:),CMJuntas(3,:),'kx')
plot3(CMJuntas(1,1),CMJuntas(2,1),0,'ro')
hold on
if Q == [0 0 0 0 0 0 0 0 0 0]
    
else
% Calculo dos momentos, em N.m:
% Momentos da Perna Esquerda:
t1L = ((m0+2*m1+2*m2+2*m3+2*m4+m5) * g * (CMJuntas(2,1) * 0.001)); 
t2L = ((m0+m1+2*m2+2*m3+2*m4+m5) * g * (CMJuntas(1,2) * 0.001));
t3L = ((m0+m1+m2+2*m3+2*m4+m5) * g * (CMJuntas(1,3) * 0.001));
t4L = ((m0+m1+m2+m3+2*m4+m5) * g * (CMJuntas(1,4) * 0.001));
t5L = ((m0+m1+m2+m3+m4+m5) * g * (CMJuntas(2,5) * 0.001));

% Momentos da Perna Direita:
t5R = ((m0+m1+m2+m3+m4) * g * (CMJuntas(2,6) * 0.001));
t4R = ((m0+m1+m2+m3) * g * (CMJuntas(1,7) * 0.001));
t3R = ((m0+m1+m2) * g * (CMJuntas(1,8) * 0.001));
t2R = ((m0+m1) * g * (CMJuntas(1,9) * 0.001));
t1R = ((m0) * g * (CMJuntas(1,10) * 0.001));

t_L = [t1L; t2L; t3L; t4L; t5L];
t_R = [t5R; t4R; t3R; t2R; t1R];

M1L = horzcat(M1L,t_L(1,1));
M2L = horzcat(M2L,t_L(2,1));
M3L = horzcat(M3L,t_L(3,1));
M4L = horzcat(M4L,t_L(4,1));
M5L = horzcat(M5L,t_L(5,1));

M5R = horzcat(M5R,t_R(1,1));
M4R = horzcat(M4R,t_R(2,1));
M3R = horzcat(M3R,t_R(3,1));
M2R = horzcat(M2R,t_R(4,1));
M1R = horzcat(M1R,t_R(5,1));
end

% DESENHO DA BASE DOS PES:
Pe_direito = [-50 ,-50 , 90 , 90 , -50 
              -35 , 35 , 35  , -35 , -35      
                0 ,  0 , 0   ,  0  ,  0 ];
Desfill3(Pe_direito,'k')
% hold on
Pe_esquerdo =[ 0    ,  0   ,  0   ,  0  
              -35   ,  35  ,  35  , -35 
              -50   , -50  , 90  , 90   
               1    ,   1  ,   1  ,   1 ];

Pe_esquerdo1 = RTG*Pe_esquerdo;
Desfill3(Pe_esquerdo1,'k')

% DESENHO DOS ELOS E JUNTAS DAS PERNAS:
 plot3(Pf(1,:),Pf(2,:),Pf(3,:),'b', 'linewidth', 1.5)    
P_inicial = [0,Pf(1,1)
             0,Pf(2,1)
             0,Pf(3,1)];
    
plot3(P_inicial(1,:),P_inicial(2,:),P_inicial(3,:),'b')
plot3(Pf(1,1:10),Pf(2,1:10),Pf(3,1:10),'go', 'linewidth', 1.5)
% Desplot3(Pf1,'b')
plot3(Pf1(1,:),Pf1(2,:),Pf1(3,:),'b', 'linewidth', 1.5)    
plot3(Pf1(1,7),Pf1(2,7),Pf1(3,7),'go', 'linewidth', 1.5)
 
axis([-250,250,-250,250,0,450])
view(45,30)
grid on
Title('Simulaçao do passo inicial do robot humanoide')
XLabel('Eixo dos XX');
YLabel('Eixo dos YY');
ZLabel('Eixo dos ZZ');  
pause(0.01)
hold off

mov(:,j) = getframe(figure(1))
end
tL = [M1L; M2L; M3L; M4L; M5L];
tR = [M5R; M4R; M3R];

% movie2avi(mov,'Cinematica_Directa_Pernas_New');

