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

close all
clc

fprintf('\t\tTeste Controlador\n');

cportreset(H);
applyjoint(H,1,0,[70 0 0]);
pause(3)
        
% Parâmetros de controlo variáveis

    carga = 454;
    
    j1_pid = 1;
    j1_ki = 10;
    j1_kp = 5;
    j1_kd = 0;
    
    error = 1;
    while(error),
        [rx,error,errorstr] = applycontrol(H,1,0,[j1_ki 0 0]);    % Especifica o integrador do controlador PID
        [rx,error,errorstr] = applycontrol(H,1,1,[j1_kp 0 0]);    % Especifica o ganho proporcional do controlador PID
        [rx,error,errorstr] = applycontrol(H,1,2,[j1_kd 0 0]);    % Especifica o derivador do controlador PID
        [rx,error,errorstr] = applycontrol(H,1,3,[j1_pid 0 0]);    % Define o controlador
    end

% inicialização de variaveis e constantes


    tick = 20/1000;         % duração de um tick na slave
    f = 50;                 % numero de frames por segundo para animação do movimento
    c = -1;                 % direcção do joelho (-1 -> lado direito; +1 -> lado esquerdo)
    velocidade = 50;       % definição da "velocidade" das juntas ---> [servo1 servo2 servo3]
    t = velocidade*tick;    % Tempo de movimento para cada servo
    applyjoint(H,1,1,[velocidade 50 50])

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

    f_j1 = figure('Units', 'normalized', 'Position', [0 0 1 1], 'MenuBar', 'none', 'Name', 'Posição da junta1', 'DoubleBuffer', 'on');
    

% inicio
                    
        teta1i = 70;
        teta1f = -70;
        [teta1, omega1, alfa1] = traject(teta1i, teta1f, t(1), f);              % calcula o trajecto para a 1ª junta

        applyjoint(H,1, 0, [-70 0 0]);
        j1=70;
        
        i = 1;
        tic
        while(i<=t*f),
                        
            [teta_real,state,rx,error,errorstr] = readjoint(H,1,0);
            
            if(isempty(teta_real)), 
                cportreset(H);
                disp(i);
                j1(i) = j1(i)
            else
                j1(i) = teta_real(1);
            end
            
            if(i>t*f)            
                teta1(i)=teta1(i-1);
            end   
            
            error_plot(teta1, j1, f_j1, t*f);
            
            i = i+1;                                                    
            drawnow;
            while(toc<i/f),        %
            end                    % responsavel pela animação com f frames por segundo
            
        end

% Gerar gráficos com o erro

        fprintf('Erro final: %i\n', abs(teta1(end)-j1(end)));
                                            
% FIM        
        
        resposta = ask;
        if(resposta == 'Sim'),
            cd Resultados\
            
            nome = ['result_carga' num2str(carga) '__J1_ki_kp_kd' num2str(j1_ki) '_' num2str(j1_kp) '_' num2str(j1_kd)];
            save(nome, 'j1');            
            hgsave(f_j1, ['junta1_carga' num2str(carga) '__J1_ki_kp_kd' num2str(j1_ki) '_' num2str(j1_kp) '_' num2str(j1_kd)])
            
            cd ../
        end
        