function [t,CoPtraj]=analyse_poly5(filename)

% Fechar todas as figuras abertas
close all

% Características dos elos e das correias de transmissão
footdata;

% Carregar ficheiro com os dados a analisar
load(filename);

% Consições da experiência
fprintf('\nCondições da Experiencia:\n');
fprintf('Pos= (%g,%g,%g) => (%g,%g,%g)\nSpeed=%d\t\tK=[%d %d %d]\n',pos0,posF,speed,K);

% Cálculo das trajectórias teóricas do CoP
CoPtraj=calctrajectory_poly5(t,pos0,posF,speed/50);

% Cálculo do Centro de Pressão com base no valor dos sensores de pressao
% medidos
CoPmeas=calccop(reaction,servo,scu);

% Conversão dos ângulos dos servos para os angulos das juntas
theta=convert2jointangles(scu,servo,Nrel);
% Cálculo da cinemática directa
% (coordenadas cartesianas do joelho e da anca)
xy=cinematdir(theta,L);

figure('name','Centro de Pressão e respectiva Cinemática Directa','Position',[50 50 1000 900])
for i=1:2
    
    subplot(3,2,i)
    plot(t,CoPtraj(:,i),'b',t,CoPmeas(:,i),'r','linewidth',2);
    grid on
    title('CoP');
    xlabel('t');
    ylabel([char('w'+i) char('w'+i)]);
    legend('Previsto','Medido');
    
    subplot(3,2,i+2)
    plot(t,CoPtraj(:,i)-CoPmeas(:,i),'b','linewidth',2);
    grid on
    title('Erro do CoP');
    xlabel('t');
    ylabel([char('w'+i) char('w'+i)]);

    % Junta do Joelho
    subplot(3,2,i+4)
    plot(t,xy(:,i,1),t,xy(:,i,2),'linewidth',2);
    grid on
    legend('Joelho','Anca');
    xlabel('t');
    ylabel([char('w'+i) char('w'+i)]);
    title('Coordenadas Cartesianas (cm)')
end

disp 'Variação do centro de pressão:'
CoPtraj(end,:)-CoPtraj(1,:)

disp 'Variação da cinemática directa (joelho):'
xy(end,:,1)-xy(1,:,1)

disp 'Variação da cinemática directa (anca):'
xy(end,:,2)-xy(1,:,2)


figure('name','Ângulos reais das juntas','Position',[50 50 1000 600]);
lgd=['Pé (ortogonal)';
     'Pé (paralelo) ';
     'Joelho        '];
for i=1:3
    subplot(3,1,i)
    plot(t,theta(:,i),'linewidth',2);
    grid on
    xlabel('t');
    ylabel('Posição (graus)');
    title(lgd(i,:));
end
