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);

scu=scu_all(1);

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

pos

% Lista de posições a atingir e escalas de tempo correspondentes
pos2=[];
for i=1:rounds
    pos2=[pos2; pos];
end
T=[t_border ones(size(t_border,1))*speed/50];

% Cálculo das trajectórias teóricas do CoP
CoPtraj=calctrajectory_poly5(t,pos2,T);

% 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=convertangles(scu,servo,-1);
% Cálculo da cinemática directa
% (coordenadas cartesianas do joelho e da anca)
xyz=cinematdir2(theta,[0 0 0]);
%xyz=xyz(1,:,end);               % Coordenadas da anca

%figure('name','Centro de Pressão e respectiva Cinemática Directa','Position',[50 50 1000 900])
for i=1:3
    
    figure('Position',[50 50 1000 650]);
    
    subplot(3,1,1)
    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,1,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,1,3)
    plot(t,xyz(:,i,4),t,xyz(:,i,5),'linewidth',2);
    grid on
    legend('Joelho','Anca');
    xlabel('t');
    ylabel([char('w'+i) char('w'+i)]);
    title('Coordenadas Cartesianas (cm)')
end
%figure
figure('Position',[50 50 400 600]);
plot(CoPmeas(:,1),CoPmeas(:,2),'.',[pos(:,1); pos(1,1)],[pos(:,2); pos(1,2)],'linewidth',2)
axis([-40 40 -70 50])
axis equal
grid on
xlabel('CoP x');
ylabel('CoP y');
legend('Trajectória medida','Trajectória prevista')
