% Movimento lateral de uma perna em malha aberta e fechada (com e sem
% carga)
%
% Caso pretenda mascarar algumas curvas, comente algumas linhas dos arrays
% DATA e LEGENDA no inicio deste script!

clear all
close all

help openVSclosed_loop

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_withcontrolki15kp50.mat';  % Este ficheiro será utilizado para prever a trajectória
      'leglat_noload_nocontrol.mat            ';
      'leglat_withload_nocontrol.mat          ';
      'leglat_withload_withcontrolki15kp50.mat'];

% Legenda das curvas
legenda=['Expected Trajectory ';
         'No Load, Open Loop  ';
         'With Load, Open Loop';
         'Ki=0.15, Kp=0.20    '];

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

% _________________________________________________________________________
% Figura 1: Dados da junta do pé
figure('name','Resposta em malha aberta e em malha fechada');
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('tempo (s)');
ylabel('Posição (graus)');
hold off
