%% Robo - Dinamica, Reacção, CoP, CoG %%

close all
clear all
clc
addpath ./functions/
fprintf('\t\tRobo - Dinamica, Reacção, CoP, CoG\n');


% inicialização de variaveis e constantes

    l1 = 1;                 % comprimento da tíbia
    l2 = 1;                 % comprimento do fémur
    l3 = 1;                 % comprimento do tronco
    r1 = l1/2;              % distancia da junta 1 ao centro de massa da tíbia
    r2 = l2/2;              % distancia da junta 2 ao centro de massa do fémur
    r3 = l3/2;              % distancia da junta 3 ao centro de massa do tronco
    m1 = 1;                 % massa da tíbia
    m2 = 1;                 % massa do fémur
    m3 = 1;                 % massa do tronco
    g = 9.8;                % aceleração da gravidade
    t = 3;                  % tempo necessário para a execução do movimento
    f = 25;                 % numero de frames por segundo para animação do moviumento
    c = -1;                 % direcção do joelho (-1 -> lado direito; +1 -> lado esquerdo)
    angulo_tibia = pi/2;    % angulo inicial que a tíbia faz com a horizontal
    angulo_femur = pi/2;    % angulo inicial que o fémur faz com a horizontal
    angulo_tronco = pi/6;   % angulo constante que o tronco faz com a horizontal
    
    Go_back = 0;            % variavel sobre o tipo de demonstração, com retorno à posição inicial ou não    
        while(Go_back~='s' & Go_back~='n')
            Go_back = input('Pretende que o robô volte sempre à posição inicial?(s,n)', 's');
        end
        if(Go_back=='s')    
            GB = 2;         % duplica o tempo de execução nos diferentes cálculos ao longo do script
        else
            GB = 1;
        end
        
    robo = zeros([2 4 GB*t*f]);                % Criação de vector de posição inicial das juntas (tronco, anca, joelho, pé)   
    robo(2,4,:) = ones(1,GB*t*f)*(2*l2/7);     % junta do pé

    L = [l1, l2, l3];       %
    R = [r1, r2, r3];       %
    M = [m1, m2, m3];       %% matrizes auxiliares
    clc
    
% criação e animação do robo

    teta1 = angulo_tibia;                   % posição angular inicial da primeira junta
    teta2 = angulo_femur - teta1;           % posição angular inicial da segunda junta
    teta3 = angulo_tronco - (teta1 + teta2);% posição angular inicial da terceira junta
    pos_ref = [ones(1,GB*t*f)*robo(1,4); ones(1,GB*t*f)*robo(2,4)];     % posição da primeira junta, que serve de referencia para toda a estrutura móvel

    [robo(:,3,1), robo(:,2,1), robo(:,1,1)] = direct_cine_3R(teta1, teta2, teta3, l1, l2, l3);     % calcula no espaço cartesiano a posição das juntas de acordo com o ângulo especificado
    robo(:,1:3,1) = robo(:,1:3,1)+pos_ref(:,1:3);            % normaliza a posição das juntas em relação à referencia -> primeira junta
    desenhar_3R(robo(:,:,1), l1, l2, l3);

    
    while(1),

        clc
        fprintf('\t\tRobo - Dinamica, Reacção, CoP, CoG\n');
        fprintf('L1 = %i;\tL2 = %i;\tL3 = %i;\n', l1,l2,l3);
        fprintf('M1 = %i;\tM2 = %i;\tM3 = %i;\n', m1, m2, m3);
        fprintf('Tempo de Execução -> %iseg\n', t*GB);

        
        [a,b] = ginput(1);                % fica a aguardar que se indique a posição final a alcançar
        while(sqrt((a-pos_ref(1,1))^2+(b-pos_ref(2,1))^2)>(l1+l2)),         % teste ao alcance do braço            
                disp('Posição fora da gama de alcance do braço');
                [a,b] = ginput(1);                
        end
         if(isempty([a,b]))     %
             break;             %
         end                    %% se for pressionado "enter" sai da simulação
        hold on                %
        plot(a, b, 'x');       %
        hold off               %% indica na simulação a posição final a atingir   
         
             
         [teta1i, teta2i] = inv_cine_2R(robo(1,2)-robo(1,4), robo(2,2)-robo(2,4), l1, l2, c);  %
         [teta1f, teta2f] = inv_cine_2R(a-robo(1,4), b-robo(2,4), l1, l2, c);                  %
         teta3i = angulo_tronco-(teta1i+teta2i);                                            %
         teta3f = angulo_tronco-(teta1f+teta2f);                                            %% cálculo da posição angular inicial e final

         [teta1, omega1, alfa1] = traject(teta1i, teta1f, t, f);              % calcula o trajecto para a 1ª junta
         [teta2, omega2, alfa2] = traject(teta2i, teta2f, t, f);              % calcula o trajecto para a 2ª junta
         [teta3, omega3, alfa3] = traject(teta3i, teta3f, t, f);              % calcula o trajecto para a 2ª junta

         if(Go_back=='s'),
             [teta1_f, omega1_f, alfa1_f] = traject(teta1f, teta1i, t, f);              % calcula o trajecto para a 1ª junta
             [teta2_f, omega2_f, alfa2_f] = traject(teta2f, teta2i, t, f);              % calcula o trajecto para a 2ª junta
             [teta3_f, omega3_f, alfa3_f] = traject(teta3f, teta3i, t, f);              % calcula o trajecto para a 2ª junta

             teta1 = [teta1, teta1_f];      %
             teta2 = [teta2, teta2_f];      %
             teta3 = [teta3, teta3_f];      %
                                            %
             omega1 = [omega1, omega1_f];   %
             omega2 = [omega2, omega2_f];   %
             omega3 = [omega3, omega3_f];   %
                                            %
             alfa1 = [alfa1, alfa1_f];      %
             alfa2 = [alfa2, alfa2_f];      %
             alfa3 = [alfa3, alfa3_f];      %% faz a duplicação dos vectores para acomodar a trajectória de retorno       
         end

         
         [mov3, mov2, mov1] = direct_cine_3R(teta1, teta2, teta3, l1, l2, l3);  % converte o movimento para o espaço cartesiano

        
        TETA = [teta1; teta2; teta3];       %
        OMEGA = [omega1; omega2; omega3];   %
        ALFA = [alfa1; alfa2; alfa3];       %% matrizes auxiliares
        
        [I, C, G, T] = DinInv3R(TETA, OMEGA, ALFA, M, L, R, g);     % cálculo do binário
        Rea = RobGRF(TETA, OMEGA, ALFA, M, L, R, g);                % cálculo da força de reacção
            Rt = Rea(1,:);                                          %% Reacção tangencial
            Rn = Rea(2,:);                                          %% Reacção normal
        CM = centro_massa_3R(TETA, M, L, R);            % cálculo do centro de massa
        CP = T(1,:)./sqrt(Rt.^2+Rn.^2);                 % cálculo do centro de pressão -> CP = T/R            
        
        mov1 = mov1 + pos_ref;      %
        mov2 = mov2 + pos_ref;      %
        mov3 = mov3 + pos_ref;      %% normalização dos vectores movimento em relação à referencia - primeira junta
        robo(:,1,:) = mov1;         %
        robo(:,2,:) = mov2;         %
        robo(:,3,:) = mov3;         %% actualização da matriz robo para a posterior execução do movimento
        
        
        % construcção da matriz com as posições do rasto em cada instante
        rasto = ones([2, GB*t*f GB*t*f]);     
        i = 1;
        while(i<length(robo(1,2,:))),
            rasto(:,1:i,i) = robo(:,2,1:i);
            i = i+1;
        end
        
        % sequencia de animação
        i = 1;
        tic
        while(i<length(robo(1,1,:))),   
            desenhar_3R(robo(:,:,i), l1, l2, l3)                        %% desenha robô
             hold on                                                                 
             plot(rasto(1,1:i,i), rasto(2,1:i,i), 'k-.');               %% desenha rasto
             
             plot(a, b, 'x');                                           %% marca a posição final
                                                                        
             plot(CM(1,i), CM(2,i), 'b+');                              %
             plot([CM(1,i) CM(1,i)], [CM(2,i) pos_ref(2,1,1)], 'b-.');  %
             plot(CM(1,i), pos_ref(2,1,1), 'b*');                       %% centro de massa e respectiva projecção
                                                                        
             plot(CP(i), pos_ref(2,1,1), 'go');                         %% marca o centro de pressão
             title('simulador');                                                     
             hold off                                                   
            i = i+1;                                                    
            while(toc<1/f),        %
                pause(1/1000)      %
            end                    %% responsavel pela animação com f frames por segundo
            tic
        end
        
        figure(gcf+1);      % cria uma janela de imagem consequente à anterior
        subplot(3,1,1)
         plot(0:1/f:(length(T(1,:))-1)/f, T(1,:), 'k');             %
         title('Binário da primeira junta');                        %
         xlabel('tempo(s)');                                        %
         ylabel('Binário(N*m)');                                    % 
         axis([0 length(T(1,:))/f min(T(1,:)) max(T(1,:))]);        %% plot do binário no braço 1 ao longo do tempo
        subplot(3,1,2);                                                         
         plot(0:1/f:(length(T(2,:))-1)/f, T(2,:), 'k');             %
         title('Binário da segunda junta');                         %
         xlabel('tempo(s)');                                        %
         ylabel('Binário(N*m)');                                    %
         axis([0 length(T(2,:))/f min(T(2,:)) max(T(2,:))]);      %% plot do binário no braço 2 ao longo do tempo
        subplot(3,1,3);                                                         
         plot(0:1/f:(length(T(3,:))-1)/f, T(3,:), 'k');             %
         title('Binário da terceira junta');                        %
         xlabel('tempo(s)');                                        %
         ylabel('Binário(N*m)');                                    %    
         axis([0 length(T(3,:))/f min(T(3,:)) max(T(3,:))]);      %% plot do binário no braço 2 ao longo do tempo
        
        figure(gcf+1);              % cria uma janela de imagem consequente à anterior
        subplot(2,1,1)
         plot(0:1/f:(length(Rt)-1)/f, Rt, 'k');                     %
         title('Reacção tangencial do plano');                      %
         xlabel('tempo(s)');                                        %
         ylabel('Força(N)');                                        % 
         axis([0 length(Rt)/f min(Rt) max(Rt)]);                    %% plot da força de reacção tangencial do plano ao longo do tempo
        subplot(2,1,2);                                                        
         plot(0:1/f:(length(Rn)-1)/f, Rn, 'k');                     %
         title('Reacção normal do plano');                          %
         xlabel('tempo(s)');                                        %
         ylabel('Força(N)');                                        %        
         axis([0 length(Rn)/f min(Rn) max(Rn)]);                    %% plot da força de reacção normal do plano ao longo do tempo

        figure(gcf-2);      % devolve a janela com o robô de simulação ao primeiro plano

        
        robo(:,:,1) = robo(:,:,end);
        
    end     % fim do ciclo infinito
        
    clc
    fprintf('fim de simulação\n');