%% Robo com tronco, trajecto no Plano Cartesiano e no Plano das Juntas %%

hold off %close all
clear all
clc
disp('       Robo - trajecto no PC ou PJ');

% inicialização de variaveis e constantes

    l1 = 4;                 % comprimento da tíbia
    l2 = 4;                 % comprimento do fémur
    l3 = 4;                 % 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;
    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)
    tipo_plano = 'PJ';      % plano cartesiano('PC') ou plano das juntas('PJ') para calculo da trajectória
    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
        
    robo = zeros([2 4 t*f]);                % Criação de vector de posição inicial das juntas (tronco, anca, joelho, pé)   
    robo(2,4,:) = ones(1,t*f)*(2*l2/7);     % junta do pé

    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,t*f)*robo(1,4); ones(1,t*f)*robo(2,4)];

    [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 necessário
    robo(:,1:3,1) = robo(:,1:3,1)+pos_ref(:,1:3);            % normaliza a posição das juntas em relação à referencia -> pé
    desenhar_3R(robo(:,:,1), l1, l2, l3);

    
    disp('       Robo - trajecto no PC ou PJ');
    disp(strcat('L1 -> ', num2str(l1)));
    disp(strcat('L2 -> ', num2str(l2)));
    disp(strcat('Tempo de Execução -> ', num2str(t), 'seg'));

    
    while(1),
        
        [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
         hold on                %
         plot(a, b, 'x');       %
         hold off               %% indica no gráfico a posição final a atingir   
         
        switch tipo_plano
            case 'PC',         
                [x, vx, ax] = traject(robo(1,2,1), a, t, f);              % calcula o trajecto segundo o eixo dos xx
                [y, vy, ay] = traject(robo(2,2,1), b, t, f);              % calcula o trajecto segundo o eixo dos yy
                [teta1, teta2] = inv_cine(x-pos_ref(1,:),y-pos_ref(2,:), l1, l2, c);       % calcula o ângulo nas juntas necessário para atingir a posição final
                teta3 = angulo_tronco*ones(1,length(teta1))-(teta1+teta2);
                [mov3, mov2, mov1] = direct_cine_3R(teta1, teta2, teta3, l1, l2, l3);
                

            case 'PJ',                
                [teta1i, teta2i] = inv_cine(robo(1,2)-robo(1,4), robo(2,2)-robo(2,4), l1, l2, c);
                [teta1f, teta2f] = inv_cine(a-robo(1,4), b-robo(2,4), l1, l2, c);
                teta3i = angulo_tronco-(teta1i+teta2i);
                teta3f = angulo_tronco-(teta1f+teta2f);
                
                [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
                
                [mov3, mov2, mov1] = direct_cine_3R(teta1, teta2, teta3, l1, l2, l3);
                
            otherwise
                disp('tipo de plano para efectuar a trajectória inválido');
        
        end
        
        TETA = [teta1; teta2; teta3];
        OMEGA = [omega1; omega2; omega3];
        ALFA = [alfa1; alfa2; alfa3];
        [I, C, G, T] = DinInv3R(TETA, OMEGA, ALFA, [m1, m2, m3], [l1, l2, l3], [r1, r2, r3], g);
        R = RobGRF(TETA, OMEGA, ALFA, [m1, m2, m3], [l1, l2, l3], [r1, r2, r3], g);
        
        mov1 = mov1 + pos_ref;      %
        mov2 = mov2 + pos_ref;      %
        mov3 = mov3 + pos_ref;      %% normalização dos vectores movimento em relação à referencia - pé
        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, t*f t*f]);     
        i = 1;
        while(i<t*f),
            rasto(:,1:i,i) = robo(:,2,1:i);
            i = i+1;
        end
        
        i = 1;
        tic
        while(i<t*f),   
            desenhar_3R(robo(:,:,i), l1, l2, l3)            %
             hold on                                        %
             plot(rasto(1,1:i,i), rasto(2,1:i,i), 'k-.');   %
             plot(a, b, 'x');                               %
             hold off                                       %
            i = i+1;                                        %% desenha braço, rasto e posição final a atingir
            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:t-1/f, T(1,:), 'k');                                         %
         title('Binário da primeira junta');                                    %
         xlabel('tempo(s)');                                                    %
         ylabel('Binário(N*m)');                                              % 
         axis([0 t 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:t-1/f, T(2,:), 'k');                                         %
         title('Binário da segunda junta');                                     %
         xlabel('tempo(s)');                                                    %
         ylabel('Binário(N*m)');                                              % 
         axis([0 t 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:t-1/f, T(3,:), 'k');                                         %
         title('Binário da terceira junta');                                   %
         xlabel('tempo(s)');                                                    %
         ylabel('Binário(N*m)');                                              % 
         axis([0 t 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:t-1/f, R(1,:), 'k');                                         %
         title('Reacção tangencial do plano');                                    %
         xlabel('tempo(s)');                                                    %
         ylabel('Força(N)');                                              % 
         axis([0 t min(R(1,:)) max(R(1,:))]);                                       % plot do binário no braço 1 ao longo do tempo
        subplot(2,1,2);                                                         
         plot(0:1/f:t-1/f, R(2,:), 'k');                                         %
         title('Reacção normal do plano');                                     %
         xlabel('tempo(s)');                                                    %
         ylabel('Força(N)');                                              % 
         axis([0 t min(R(2,:)) max(R(2,:))]);                                       % plot do binário no braço 2 ao longo do tempo

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

        
        robo(:,:,1) = robo(:,:,end);
        
    end     % fim do ciclo infinito