 
%--------------------------------------------------------------------------------------------
%                             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

DHPERNAS
PERNAS2

% % DEFINIÇAO DA POSIÇAO INICIAL DAs PERNAS:
% 
% Qi = [0 0 0 0 0, 0 0 0 0 0];              % <------------- Angulos em graus                                          
% 
% % DEFINIÇAO DA POSIÇAO FINAL DAS PERNAS:
% 
% % mexre os tronncos
% % Qf = [5 20 -40 20 -5 5 -30 40 -10 -5];     % <------------- Angulos em graus
% % Qf = [10 0 0 0 -10 10 0 0 0 -10];   
% 
% Qf = [0 10 -30 20 0, 0 0 20 -20 0];                                                                                   %1
% % INVOCAÇAO DA FUNÇAO ROBPLOT QUE REALIZA O DESENHO E ANIMAÇAO DA POSIÇAO E ORIENTAÇAO DAS PERNAS:
% [CMJuntas_i,CMTotal,tL,tR,mov1] = ROBPLOT(Duas_Pernas,Perna_Tronco,Qi,Qf)
% 
%  pause(3)
% 
% % % % Qf1 = [5 0 0 10 5 0 -10 20 -10 0]; % <------------- Angulos em graus
% 
% 
% Qf1 = [10 10 -30 20 -10, 10 0 20 -20 -10];                                                                             %2
% 
% [CMJuntas_1,CMTotal,tL,tR,mov2] = ROBPLOT(Duas_Pernas,Perna_Tronco,Qf,Qf1)
% 
% pause(3)
% q1l_variacao=Qf(1):Qf1(1)/10:Qf1(1) 
% q2l_variacao=Qf(2):Qf1(2)/10:Qf1(2) 
% q3l_variacao=Qf(3):Qf1(3)/10:Qf1(3) 
% q4l_variacao=Qf(4):Qf1(4)/10:Qf1(4) 
% q5l_variacao=Qf(5):Qf1(5)/10:Qf1(5) 
% 
% Qf2 = [10 10 -30 20 -10, 10 -40 50 -10 -10];                                                                           %3
% 
% [CMJuntas_2,CMTotal,tL,tR,mov3] = ROBPLOT(Duas_Pernas,Perna_Tronco,Qf1,Qf2)  
%   
% pause(3)
% 
% Qf3 = [10 20 -20 0 -10, 10 -20 30 -10 -10];                                                                                  %4 
%  
% [CMJuntas_3,CMTotal,tL,tR,mov4] = ROBPLOT(Duas_Pernas,Perna_Tronco,Qf2,Qf3) 
% 
% pause(3)
%  
% Qf4 = [0 20 -20 0 0, 0 -20 30 -10 0];                                                                         
%  
% [CMJuntas_4,CMTotal,tL,tR,mov5] = ROBPLOT(Duas_Pernas,Perna_Tronco,Qf3,Qf4)                                                            %5 
% 
% pause(2)
% 
% %%%%%%%%-----------------------------------------------------%%%%%%%%%%%%%%%%%%%
% % t = 0:1:10;
% % 
% % plot(t,q1l_variacao,'r')
% % 
% % hold on
% % plot(t,q2l_variacao,'g')
% % plot(t,q3l_variacao,'b')
% % plot(t,q4l_variacao,'y')
% % plot(t,q5l_variacao,'k')
% 
% %%%%%%%%-----------------------------------------------------%%%%%%%%%%%%%%%%%%%
% movie2avi([mov2,mov3,mov4,mov5],'Passo_INICIAL');
% % 
% % figure
% % 
% % qtotal_1 =[Qf(10) Qf1(10) Qf2(10) Qf3(10) Qf4(10)]
% % 
% % t = 0:1:4;
% % 
% % plot(t,qtotal_1,'b')
% % axis([0,10,-50,50])
% % title('Variação de ângulos de junta da perna de avanço')
% % xlabel('Posição');
% % ylabel('Ângulo(º)');
% % 
% % hold 
% % qtotal_2 =[Qf(9) Qf1(9) Qf2(9) Qf3(9) Qf4(9)]
% % plot(t,qtotal_2,'r')
% %  
% % hold on
% % qtotal_3 =[Qf(8) Qf1(8) Qf2(8) Qf3(8) Qf4(8)] 
% % plot(t,qtotal_3 ,'g')
% % 
% % hold on
% % qtotal_4 =[Qf(7) Qf1(7) Qf2(7) Qf3(7) Qf4(7)]
% % plot(t,qtotal_4,'m')
% % 
% % hold on 
% % qtotal_5 =[Qf(6) Qf1(6) Qf2(6) Qf3(6) Qf4(6)]
% % plot(t,qtotal_5,'y')
% % 
% % legend('q1','q2','q3','q4','q5')
% % pause
% % %%%%%%%%%%%------------------------------------
% % 
% % 
% % figure
% % 
% % qtotal_1 =[Qf(1) Qf1(1) Qf2(1) Qf3(1) Qf4(1)]
% % 
% % t = 0:1:4;
% % 
% % plot(t,qtotal_1,'b')
% % axis([0,10,-50,50])
% % title('Variação de ângulos de junta da perna de suporte ')
% % xlabel('Posição');
% % ylabel('Ângulo(º)');
% % 
% % hold 
% % qtotal_2 =[Qf(2) Qf1(2) Qf2(2) Qf3(2) Qf4(2)]
% % plot(t,qtotal_2,'r')
% %  
% % hold on
% % qtotal_3 =[Qf(3) Qf1(3) Qf2(3) Qf3(3) Qf4(3)] 
% % plot(t,qtotal_3 ,'g')
% % 
% % hold on
% % qtotal_4 =[Qf(4) Qf1(4) Qf2(4) Qf3(4) Qf4(4)]
% % plot(t,qtotal_4,'m')
% % 
% % hold on 
% % qtotal_5 =[Qf(5) Qf1(5) Qf2(5) Qf3(5) Qf4(5)]
% % plot(t,qtotal_5,'y')
% % 
% % legend('q1','q2','q3','q4','q5')
% % pause
% % 
% % 
% % 
% % 
% % %%%%------------------------------------------------------------------------------
% % %%%Grafico do Centro de Massa%%%%%%%%
% % 
% % figure 
% % 
% % CM_x=[CMJuntas_i(1,1) CMJuntas_1(1,1) CMJuntas_2(1,1) CMJuntas_3(1,1) CMJuntas_4(1,1)]
% % CM_y=[CMJuntas_i(2,1) CMJuntas_1(2,1) CMJuntas_2(2,1) CMJuntas_3(2,1) CMJuntas_4(2,1)]
% % 
% % 
% % Pe_direito = [-80 ,-80 , 150 , 150, -80 
% %               -50 , 50 , 50  , -50, -50 ];
% % fill(Pe_direito(1,:),Pe_direito(2,:),'k')
% % 
% % hold on
% % 
% % Pe_esquerdo =[-80   , -80  , 150  , 150  , -80
% %               75   ,  175  ,  175  , 75  , 75];
% % plot(Pe_esquerdo(1,:),Pe_esquerdo(2,:),'k--')
% % 
% % 
% % hold on 
% % plot(CM_x,CM_y,'r-o');
% % axis([-200,200,-200,200])
% % grid on 
% % title('Variação do Centro de Massa para o Passo Normal')
% % xlabel('Eixo X');
% % ylabel('Eixo Y');
% % 
% % 
% % 
% % 
% DEFINIÇAO DA POSIÇAO INICIAL DAs PERNAS:

Qi = [0 0 0 0 0, 0 0 0 0 0];              % <------------- Angulos em graus                                            

% DEFINIÇAO DA POSIÇAO FINAL DAS PERNAS:

% mexre os tronncos
% Qf = [5 20 -40 20 -5 5 -30 40 -10 -5];     % <------------- Angulos em graus
% Qf = [10 0 0 0 -10 10 0 0 0 -10];   

Qf = [0 0 0 0 0, 0 0 0 0 0];                                                                                  %1
% INVOCAÇAO DA FUNÇAO ROBPLOT QUE REALIZA O DESENHO E ANIMAÇAO DA POSIÇAO E ORIENTAÇAO DAS PERNAS:
[CMJuntas_i,CMTotal,tL,tR,mov1] = ROBPLOT(Duas_Pernas,Perna_Tronco,Qi,Qf)
pause(3)

% % % Qf1 = [5 0 0 10 5 0 -10 20 -10 0]; % <------------- Angulos em graus


Qf1 = [10 0 0 0 -10, 10 0 0 0 -10];                                                                            %2

[CMJuntas_1,CMTotal,tL,tR,mov2] = ROBPLOT(Duas_Pernas,Perna_Tronco,Qf,Qf1)

 pause(3)

Qf2 = [10 0 0 0 -10, 10 -30 40 -10 -10];                                                                           %3

[CMJuntas_2,CMTotal,tL,tR,mov3] = ROBPLOT(Duas_Pernas,Perna_Tronco,Qf1,Qf2)  
%   
pause(3)

Qf3 = [10 20 -20 0 -10, 10 -20 30 -10 -10];                                                                        %4

[CMJuntas_3,CMTotal,tL,tR,mov4] = ROBPLOT(Duas_Pernas,Perna_Tronco,Qf2,Qf3) 
% 
pause(3)
% 
% 
Qf4 = [0 20  -20 0 0 , 0 -20 30 -10 0];                                                                        %5

[CMJuntas_4,CMTotal,tL,tR,mov5] = ROBPLOT(Duas_Pernas,Perna_Tronco,Qf3,Qf4) 

%%%%%%%%%______________________________________