% Movimento de flexão de uma perna com compensação PI
%
% Respostas para vários conjuntos de parametros!
%
% Caso pretenda mascarar algumas curvas, comente algumas linhas dos arrays
% DATA e LEGENDA no inicio deste script!

clear all
close all

help testing_kp

TIME_EXTRA=3/0.02;

% Loading dos dados
% O primeiro ficheiro apenas será utilizado para prever a trajectória a ser
% executada. É importante, por isso, que seja com o controlador activo pois
% já possui os valores finais corrigidos!

data=['bothlegs_withload_withcontrolki5.mat     ';  % Este ficheiro será utilizado para prever a trajectória
      'bothlegs_withload_nocontrol.mat          ';
      'bothlegs_withload_withcontrolki5.mat     ';
      'bothlegs_withload_withcontrolki5kp20.mat ';
      'bothlegs_withload_withcontrolki5kp50.mat ';
      'bothlegs_withload_withcontrolki5kp100.mat';
      'bothlegs_withload_withcontrolki5kp150.mat'];

% Legenda das curvas
legenda=['Expected       ';
         'Open Loop      ';
         'Ki=0.05 Kp=0.00';
         'Ki=0.05 Kp=0.20';
         'Ki=0.05 Kp=0.50';
         'Ki=0.05 Kp=1.00';
         'Ki=0.05 Kp=1.50'];

% Estilo das curvas
style=['k:';
       'b ';
       'r ';
       'm ';
       'g ';
       'c ';
       'y ';
       'k '];

% _________________________________________________________________________
% Figura 1: Dados da junta do pé
for g=1:3
    
    % SCU 1, junta 2
    if (g==1)
        titl='Resposta da junta do pé com compensação PI';
        index=1;
        scu=1;
        joint=2;
    % SCU 1, junta 3
    elseif (g==2)
        titl='Resposta da junta do joelho com compensação PI';
        index=1;
        scu=1;
        joint=3;
    % SCU 3, junta 1
    else
        titl='Resposta da junta da anca com compensação PI';
        index=2;
        scu=3;
        joint=1;
    end

    figure('name',titl);
    axes('fontsize',14);
    grid on
    hold on

    % Desenho das diversas curvas
    for k=1:size(data,1)
        for i=1:length(data(k,:))
            if (data(k,i)==' '), i=i-1; break, end
        end
        % Carregando dados
        d=load(data(k,1:i));
       % Tamanho do array de dados
        L=length(d.time);
        % Array tempo
        time=d.time;
 
        % Se for a primeira curva, prever a trajectória
        if (k==1)
            pos=predict_trajectory(d.position(index,joint,1),d.position_ref(scu,joint),L,d.time_ref/(d.time_ref+TIME_EXTRA));
        % Caso contrário, visualizar os dados
        else
            pos=zeros(1,L);
            for i=1:L, pos(i)=d.position(index,joint,i); end
        end

        % Visualização gráfica
        plot(time,pos,style(rem(k-1,length(style))+1,:),'linewidth',2);
    end

    % Visualização da legenda
    legend(legenda);
    xlabel('time (s)');
    ylabel('position (degrees)');
    hold off
end