%%          Braço Robótico comtrajectória definida no PC ou PJ          %%

hold off %close all
clear all
clc
disp('      Braço Robótico - Plano Cartesiano ou Plano Juntas');

% inicialização de variaveis e constantes

    l1 = 1;                 % comprimento da primeira secção do braço
    l2 = 1;                 % comprimento da segunda secção do braço
    t = 3;                  % tempo necessário para a execução do movimento
    f = 25;                 % numero de frames por segundo
    braco = zeros(2,3);     % Criação da matriz com de posição inicial das juntas
    tipo_plano = input('Defina o plano em que pretende traçar a trajectória:\nPC - Plano Cartesiano\nPJ - Plano das Juntas\n->', 's');      % ou 'PJ' -> plano cartesiano ou plano das juntas
    c = 1;                  % cotovelo para cima ou para baixo
    clc;

% criação e animação do braço

    [teta1, teta2] = inv_cine(0, 2*l2, l1, l2, c);
    [braco(:,2), braco(:,3)] = direct_cine(teta1, teta2, l1, l2);     % calcula no espaço cartesiano a posição das juntas de acordo com o ângulo necessário
    desenhar(braco, l1, l2)

    disp('      Braço Robótico - Plano Cartesiano ou Plano Juntas');
    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(a^2+b^2>(l1+l2)^2),         % 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                                   %% coloca a marca da posição final
        
        switch tipo_plano
            case 'PC',
                x = traject(braco(1,3), a, t, f);              % calcula o trajecto segundo o eixo dos xx
                y = traject(braco(2,3), b, t, f);              % calcula o trajecto segundo o eixo dos yy

                [teta1, teta2] = inv_cine(x, y, l1, l2, c);
                [mov2, mov3] = direct_cine(teta1, teta2, l1, l2);
                

            case 'PJ',
                [teta1i, teta2i] = inv_cine(braco(1,3), braco(2,3), l1, l2, c);
                [teta1f, teta2f] = inv_cine(a, b, l1, l2, c);
                
                teta1 = traject(teta1i, teta1f, t, f);              % calcula o trajecto para a 1ª junta
                teta2 = traject(teta2i, teta2f, t, f);              % calcula o trajecto para a 2ª junta
                
                [mov2, mov3] = direct_cine(teta1, teta2, l1, l2);
                
            otherwise
                disp('tipo de plano para efectuar a trajectória inválido');
        end    
 
        
        rasto = [braco(1,3); braco(2,3)];
        i = 1;
        tic
        while(i<t*f),
            braco(:,2:3) = [mov2(:,i) mov3(:,i)];       % actualiza posição do braço
            rasto = [rasto mov3(:,i)];                  %
            desenhar(braco, l1, l2)                     %
             hold on                                    %
             plot(rasto(1,:), rasto(2,:), 'k-.');       %
             plot(a, b, 'x');                           %
             hold off                                   %% desenha braço, rasto e posição final a atingir
            i = i+1;    
            while(toc<1/f),        %
                pause(1/1000)      %
            end                    % responsavel pela animação com f frames por segundo
            tic
        end
    end
    
    clear all
    clc

