%--------------------------------------------------------------------------------------------
%                             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
%--------------------------------------------------------------------------------------------
%
% Programa principal PROG_PRINCIPAL: Neste programa realiza-se uma simulaçao em ambiente MatLab,
%                                    que nos permite visualizar a cinematica directa das pernas 
%                                    da plataforma humanoide. Aqui tambem iremos calcular os 
%                                    momentos(torques) associados a cada junta, bem como os centros de massa 
%                                    dos diferentes elos.
% clear all
% close all
clc
warning off

% Scripts com os dados das cadeias cinematicas
DHPERNAS
PERNA_TRONCO

pp=Q*180/pi;

TTL = [];
TTL = [];

for i = 1 : size(Q,2)-1
% DEFINIÇAO DA POSIÇAO INICIAL DAs PERNAS:
Qi = [pp(1,i) pp(2,i) pp(3,i) pp(4,i) pp(5,i) pp(8,i) -pp(9,i) abs(pp(10,i)) -pp(11,i) pp(12,i)]              % <------------- Angulos em graus

% DEFINIÇAO DA POSIÇAO FINAL DAS PERNAS:
Qf = [pp(1,i+1) pp(2,i+1) pp(3,i+1) pp(4,i+1) pp(5,i+1) pp(8,i+1) -pp(9,i+1) abs(pp(10,i+1)) -pp(11,i+1) pp(12,i+1)]              % <------------- Angulos em graus



% INVOCAÇAO DA FUNÇAO ROBPLOT QUE REALIZA O DESENHO E ANIMAÇAO DA POSIÇAO E
% ORIENTAÇAO DAS PERNAS: 
[CMJuntas,CMTotal,tL,tR,CML,CMR,Pf] = ROBPLOT(Duas_Pernas,Perna_Tronco,Qi,Qf)

mov(:,i) = getframe(figure(1))

TTL(:,i) = tL(:,2);
TTR(:,i) = tR(:,2);
 pause(0.05)
end

movie2avi(mov,'Cinematica_Directa_Pernas');

figure(2)
hold on
grid on
t = 0:1.2/49:1.2;
cor = 'bgrcm'
for j = 1:5
    plot(t,[TTL(2,1:24),TTL(2,25:50).*-1],'g','linewidth',3)
    plot(t,[TTL(4,1:15),TTL(4,16:50).*-1],'c','linewidth',3)
    plot(t,TTL(1,1:50),'b','linewidth',3)
    plot(t,TTL(3,1:50),'r','linewidth',3)
    plot(t,TTL(5,1:50),'m','linewidth',3)
    
end
    xlabel('Tempo de Simulaçao, segundos')
    ylabel('Binarios Perna de Suporte, N.m')