%%      Implementação da matriz Jacobiano com realimentação      %%

close all
clear all
clc
addpath ./functions/
fprintf('\t\tRobo - Implementação da Matriz jacobiano com realimentação\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
    m1 = 1;                 % massa da tíbia
    m2 = 1;                 % massa do fémur
    g = 9.8;                % aceleração da gravidade
    t = 3;                  % tempo de execução do movimento
    ts = 1e-3;              % incremento de tempo para os cálculos de trajectória
    f = 25;                 % numero de imagens por segundo para animação do moviumento
    c = -1;                 % direcção do joelho (-1 -> lado direito; +1 -> lado esquerdo)
    angulo_tibia = pi/2;    % angulo que a tíbia faz com a horizontal
    angulo_femur = pi/2;    % angulo que o fémur faz com a horizontal
    angulo_tronco = pi/2;   % angulo que o tronco faz com a horizontal
    
    kp = [300 0;0 300];     % matriz ganho para o cálculo da velocidade angular com realimentação
    
    robo = zeros([2 4 t/ts]);    % Criação de vector de posição inicial das juntas (tronco, anca, joelho, pé)   
    robo(2,4,:) = ones(1,t/ts)*(2*l2/7);  % junta do pé
    
% 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/ts)*robo(1,4); ones(1,t/ts)*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);
    title('simulador');

    while(1),
        
        clc
        fprintf('\t\tRobo - Implementação da Matriz jacobiano com realimentação\n');
        fprintf('L1 = %i;\tL2 = %i;\tL3 = %i;\n', l1,l2,l3);
        fprintf('M1 = %i;\tM2 = %i;\n', m1, m2);
        fprintf('ganho definido na realimentação =  %i\n', kp(1,1));
        fprintf('precisão do vector tempo para os cálculos -> %f\n', ts);
        fprintf('Tempo de Execução -> %iseg\n', t);

        
        [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 da anca            
                fprintf('\nPosição fora da gama de alcance do braço\n');
                [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   
         
                 
        [x, vx, ax] = traject(robo(1,2,1), a, t, 1/ts);              % calcula o trajecto segundo o eixo dos xx
        [y, vy, ay] = traject(robo(2,2,1), b, t, 1/ts);              % calcula o trajecto segundo o eixo dos yy
        [teta1, teta2] = inv_cine_2R(x-pos_ref(1,:), y-pos_ref(2,:), l1, l2, c); %inv_cine_2R(x(1)-pos_ref(1,1), y(1)-pos_ref(2,1), l1, l2, c); %robo(1,2,1),robo(2,2,1), l1, l2, c);       % calcula o ângulo nas juntas necessário para atingir a posição final

        teta = [teta1; teta2];
        teta_c = teta;
        for k=1:t/ts-1,
            jacobiano = gera_jacobiano(teta(1,k), teta(2,k), l1, l2);
            
            %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            jacobiano2 = gera_jacobiano(teta_c(1,k), teta_c(2,k), l1, l2);  %
            if(det(jacobiano)==0)                                           %
              fprintf('\n\tErro -> Matriz jacobiano singular\n');           %
            else                                                            %
              omega_c(:,k) = inv(jacobiano2)*[vx(k); vy(k)];                %
              teta_c(:,k+1) = teta_c(:,k) + omega_c(:,k)*ts;                %
            end                                                             %
            %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% método anterior de cálculo da posição angular - executado para comparar o erro

            [j2, j3] = direct_cine_2R(teta(1,k), teta(2,k), l1, l2);   %
            j3 = j3 + pos_ref(:,1);                                 %% obtenção da posição da junta 2 para o cálculo do erro
            e = [x(k); y(k)] - j3;                                  % cálculo do vector erro
            omega(:, k) = jacobiano'*kp*e;                          % cálculo do vector velocidade angular através da realimentação
            teta(:,k+1) = teta(:,k) + omega(:,k)*ts;                % cálculo da posição angular através de integração
              
        end

        
        teta1 = teta(1,:);
        teta2 = teta(2,:);      
        teta3 = angulo_tronco*ones(1,length(teta1))-(teta1+teta2);
        [mov3, mov2, mov1] = direct_cine_3R(teta1, teta2, teta3, l1, l2, l3);
        
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        [mov1_c, mov2_c] = direct_cine_2R(teta_c(1,:), teta_c(2,:), l1, l2);   %
        mov2_c = mov2_c + pos_ref;                                          %
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% movimento de comparação
        
        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 = robo(:,2,1:1/(f*ts):t/ts);

        % cálculo do erro em cada instante
        erro = sqrt((x-mov2(1,:)).^2+(y-mov2(2,:)).^2);     % distancia euclidiana entre a posição desejada e a posição real
        erro_c = sqrt((x-mov2_c(1,:)).^2+(y-mov2_c(2,:)).^2);     % distancia euclidiana entre a posição desejada e a posição real
        
        
        % sequencia de animação
        i = 1;
        tic
        while(i<t*f),

            desenhar_3R(robo(:,:,i/(f*ts)), l1, l2, l3)     %
             hold on                                        %
             plot(rasto(1,1:i), rasto(2,1:i), 'k-.');       %   
             plot(a, b, 'x');                               %
             title('simulador');                            %
             hold off                                       %% desenha braço, rasto e posição final a atingir

            i = i+1;
            while(toc<1/f),         %
                pause(1/1000)       %
            end                     % responsável pela animação com f imagens por segundo
            tic
        end

        figure(gcf+1);      % cria uma janela de imagem consequente à anterior
         plot(0:ts:t-ts, erro, 'k', 0:ts:t-ts, erro_c, 'r');                    %
         title('Erro - comparação');                                            %
         xlabel('tempo(s)');                                                    %
         ylabel('variação');                                                    % 
         legend('erro com compensação', 'erro sem compensação');                %
         axis([0 t min([erro, erro_c]) max([erro, erro_c])]);                   %% plot do erro
        
        figure(gcf-1);      % devolve o primeiro plano à janela com o robô de simulação
        
        robo(:,:,1) = robo(:,:,end);    % actualiza a nova posição inicial do robô
        
    end     % fim do ciclo infinito
    
    clc
    fprintf('fim de simulação\n');