% Movimento lateral de uma perna com compensação ID
%
% 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_kd

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=['leglat_withload_withcontrolki7.mat    ';  % Este ficheiro será utilizado para prever a trajectória
      'leglat_withload_nocontrol.mat         ';
      'leglat_withload_withcontrolki7.mat    ';
      'leglat_withload_withcontrolki7kd10.mat';
      'leglat_withload_withcontrolki7kd20.mat';
      'leglat_withload_withcontrolki7kd50.mat';
      'leglat_withload_withcontrolki7kd80.mat'];

% Legenda das curvas
legenda=['Expected       ';
         'Open Loop      ';
         'Ki=0.07 Kd=0.00';
         'Ki=0.07 Kd=0.10';
         'Ki=0.07 Kd=0.20';
         'Ki=0.07 Kd=0.50';
         'Ki=0.07 Kd=0.80'];

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

% _________________________________________________________________________
% Figura 1: Dados da junta do pé
close all
figure('name','Resposta em malha fechada com compensação ID');
axes('fontsize',14);
grid on
hold on

% SCU 1, junta 1
index=1;
scu=1;
joint=1;

% 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
