%% teste do controlador de posição proporcional %%

close all
clear teta1 teta2 teta3 j1 j2 j3 robo robo_real
clc
fprintf('\t\tTeste\n');

% applyjoint(H,2,0,[-20 20 0]);


velocidade = [25 10 100];       % definição da "velocidade" das juntas ---> [servo1 servo2 servo3]
        
% Parâmetros de controlo variáveis

    perna = 'direita';
    
    
    j1_ki = 0;
    j1_kp = 0;
    j1_kd = 0;

    j2_ki = 0;
    j2_kp = 0;
    j2_kd = 0;

    j3_ki = 0;
    j3_kp = 0;
    j3_kd = 0;
    
    
%     error = 1;
%     while(error),
%         [rx,error,errorstr,tries] = applycontrol(H,2,0,[j1_ki j2_ki j3_ki]);        % Especifica o ganho proporcional do controlador PID
%         [rx,error,errorstr,tries] = applycontrol(H,2,1,[j1_kp j2_kp j3_kp]);        % Especifica o parametro integrativo do controlador PID
%         [rx,error,errorstr,tries] = applycontrol(H,2,2,[j1_kd j2_kd j3_kd]);        % Especifica o parametro derivativo do controlador PID
%         [rx,error,errorstr,tries] = applycontrol(H,2,3,[1 1 1]);                    % Activa o controlador PID
%         [off_set_juntas,state,rx,error,errorstr,tries] = ler_pos(H,2,perna);        % Verifica o off_set das juntas da perna -> A perna deve estar na vertical
%         [velocidade,state,rx,error,errorstr,tries] = readjoint(H,2,1);              % Verifica a velocidade de cada junta -> Este valor serve para calcular a duração da simulação
%     end
off_set_juntas = [90 0 0];

% inicialização de variaveis e constantes


    l1 = 9.5;               % comprimento da tíbia
    l2 = 9.5;               % comprimento do fémur
    l3 = 6.8;               % comprimento do tronco
    h_base = 6.2;           % distancia da base do pé à primeira junta
    tick = 20/1000;         % duração de um tick na slave
    f = 10;                 % 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/2;   % angulo constante que o tronco faz com a horizontal
    angulo_flexao = 60;     % angulo da junta 1 em graus em relação à superficie
    
    t = velocidade*tick;    % Tempo de movimento para cada servo
    
    flecte_t1 = deg2rad(angulo_flexao);                 % angulo da junta 1 para flectir a perna
    flecte_t2 = pi-2*flecte_t1;                         % angulo da junta 2 para flectir a perna
    flecte_t3 = angulo_tronco-flecte_t2-flecte_t1;      % angulo da junta 3 para flectir a perna com o tronco sempre direito

    robo = zeros([2 4 1]);    % Criação de vector de posição inicial das juntas (tronco, anca, joelho, pé)   
    robo(2,4,1) = h_base;     % junta do pé

    robo_real = zeros([2 4 1]);    % Criação de vector de posição inicial das juntas (tronco, anca, joelho, pé)   
    robo_real(2,4,1) = h_base;     % junta do pé

    
% Criação da zona de construcção dos plots

    f_j1 = figure('Units', 'normalized', 'Position', [0 0.5, 0.5 0.5], 'MenuBar', 'none', 'Name', 'Posição da junta1', 'DoubleBuffer', 'on');
    f_j2 = figure('Units', 'normalized', 'Position', [0.5 0.5, 0.5 0.5], 'MenuBar', 'none', 'Name', 'Posição da junta2', 'DoubleBuffer', 'on');
    f_j3 = figure('Units', 'normalized', 'Position', [0 0 0.5 0.5], 'MenuBar', 'none', 'Name', 'Posição da junta3', 'DoubleBuffer', 'on');
    f_sim = figure('Units', 'normalized', 'Position', [0 0 1 1], 'MenuBar', 'none', 'Name', 'Simulação', 'DoubleBuffer', 'on');
    
% criação e animação do robo

    % robo virtual
        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 = robo(:,4,1);

        [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)+[ones(1,3)*pos_ref(1); ones(1,3)*pos_ref(2)];            % normaliza a posição das juntas em relação à referencia -> pé
        desenhar_3R_2D(robo(:,:,1), l1, l2, l3, f_sim, 'r');
       
    % robo real
        [robo_real(:,3,1), robo_real(:,2,1), robo_real(:,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_real(:,1:3,1) = robo_real(:,1:3,1)+[ones(1,3)*pos_ref(1); ones(1,3)*pos_ref(2)];            % normaliza a posição das juntas em relação à referencia -> pé
        desenhar_3R_2D(robo_real(:,:,1), l1, l2, l3, f_sim, 'g');
        legend('Robo Virtual', 'Robo Real');
        
        
% inicio
        
        %         [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]))     %
        %              close all;         %
        %              break;             %
        %          end                    %% se for pressionado "enter" sai da simulação
        %         hold on                %
        %         plot(a, b, 'x');       %
        %         hold off               %% indica no gráfico a posição final a atingir   
                    
        teta1i = teta1;
        teta2i = teta2;
        teta3i = teta3;
        
        teta1f = flecte_t1;
        teta2f = flecte_t2;
        teta3f = angulo_tronco-(teta1f+teta2f);

            [a,b,c] = direct_cine_3R(teta1f, teta2f, angulo_tronco-(teta1f+teta2f), l1, l2, l3);
            a=b(1)+pos_ref(1);
            b=b(2)+pos_ref(2);
            hold on
            plot(a, b, 'x');
            hold off
        
        [teta1, omega1, alfa1] = traject(teta1i, teta1f, t(1), f);              % calcula o trajecto para a 1ª junta
        [teta2, omega2, alfa2] = traject(teta2i, teta2f, t(2), f);              % calcula o trajecto para a 2ª junta
        [teta3, omega3, alfa3] = traject(teta3i, teta3f, t(3), f);              % calcula o trajecto para a 3ª junta

        % normalizar a dimensão dos vectores de movimento
        teta1 = [teta1  teta1(end)*ones(1,(max(t)-t(1))*f)];
        teta2 = [teta2  teta2(end)*ones(1,(max(t)-t(2))*f)];
        teta3 = [teta3  teta3(end)*ones(1,(max(t)-t(3))*f)];
        
        [mov3, mov2, mov1] = direct_cine_3R(teta1, teta2, teta3, l1, l2, l3);

        t = max(t); % todos os vectores de movimento já estão normalizados para o tempo máximo de execução
        mov1 = mov1 + [ones(1,t*f).*pos_ref(1,1); ones(1,t*f).*pos_ref(2,1)];      %
        mov2 = mov2 + [ones(1,t*f).*pos_ref(1,1); ones(1,t*f).*pos_ref(2,1)];      %
        mov3 = mov3 + [ones(1,t*f).*pos_ref(1,1); ones(1,t*f).*pos_ref(2,1)];      %% normalização dos vectores movimento em relação à referencia - pé
        
        
        %% actualização da matriz robo para a posterior execução do movimento
        for i=1:t*f,            
            robo(:,1,i) = mov1(:,i);
            robo(:,2,i) = mov2(:,i);
            robo(:,3,i) = mov3(:,i);
            robo(:,4,i) = robo(:,4,1);
        end
        
        
        % construcção da matriz com as posições do rasto em cada instante
        rasto = ones(2, t*f);
        rasto(:,:) = robo(:,2,:);
            
        flecte = rad2deg([teta1f teta2f teta3(end)])+off_set_juntas;
%         apply_pos(H,2,perna,flecte);        
        i = 1;
        tic
        while(i<=t*f),   
            
%             error=1;
%             while(error)
%                 [pos_real,state,rx,error,errorstr,tries] = ler_pos(H,2,perna);                    
%             end
%             pos_real = deg2rad(pos_real-off_set_juntas);
            pos_real = [teta1(i) teta2(i) teta3(i)];
            j1(i) = pos_real(1);
            j2(i) = pos_real(2);
            j3(i) = pos_real(3);
            [t1, t2, t3] = direct_cine_3R(j1(i), j2(i), j3(i), l1, l2, l3);
            
            robo_real(:,4,i) = pos_ref;
            robo_real(:,3,i) = t1+pos_ref;
            robo_real(:,2,i) = t2+pos_ref;
            robo_real(:,1,i) = t3+pos_ref;
            
                        
            desenhar_3R_2D(robo(:,:,i), l1, l2, l3, f_sim, 'r')         %
            hold on                                                     %
            desenhar_3R_2D(robo_real(:,:,i), l1, l2, l3, f_sim, 'g')    %
            hold on                                                     %
             plot(rasto(1,1:i), rasto(2,1:i), 'k-.');                   %
             plot(a, b, 'x');                                           %
             hold off                                                   %
            legend('Robo Virtual', 'Robo Real');                        %% confrontação entre o movimento desejado e o movimento real
            
            
            i = i+1;                                                    
            while(toc<i/f),        %
            end                    % responsavel pela animação com f frames por segundo
        end

% Gerar gráficos com o erro

        error_plot(rad2deg(teta1), rad2deg(j1), f_j1);
        error_plot(rad2deg(teta2), rad2deg(j2), f_j2);
        error_plot(rad2deg(teta3), rad2deg(j3), f_j3);
        
        set(f_sim, 'Position', [0.5 0 0.5 0.5]);

        fprintf('Erro final:\n\n\tjunta 1 = %gº\n\tjunta 2 = %gº\n\tjunta 3 = %gº\n', ...
                                            abs(rad2deg(teta1(end)-j1(end))), abs(rad2deg(teta2(end)-j2(end))), abs(rad2deg(teta3(end)-j3(end))));
                                            
% FIM        
        robo(:,:,1) = robo(:,:,end);
        pause(1)
        
%         apply_pos(H,2,perna,off_set_juntas);
