function [t,CoPtraj]=analyse_poly5(filename)

% Fechar todas as figuras abertas
close all

% Caracteristicas dos elos e das correias de transmiss�o
footdata;

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

% Consdicoes da experiencia
fprintf('\nCondicoes da Experiencia:\n');
%fprintf('Speed=%d\t\tK=[%d %d %d]\n',speed,Ki(1),Kp(1),K(1));

pos
scu

% Lista de posicoes 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];

% Calculo das trajectorias teoricas do CoP
CoPtraj=calctrajectory_poly5(t,pos2,T);

% Calculo do Centro de Pressao com base no valor dos sensores de pressao
% medidos
CoPmeas=calccop(reaction,servo,scu);

% Conversao dos angulos dos servos para os angulos das juntas
theta=convertangles(scu,servo,-1);
% Calculo da cinematica directa
% (coordenadas cartesianas do joelho e da anca)
xyz=cinematdir2(theta,[0 0 0])*10;
%xyz=xyz(1,:,end);               % Coordenadas da anca

for i=1:3
    
    figure('Position',[50 50 800 450]);
    
    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('Predicted Trajectory','Measured Trajectory');
    
    subplot(3,1,2)
    plot(t,CoPtraj(:,i)-CoPmeas(:,i),'b','linewidth',2);
    grid on
    title('CoP Error');
    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('Knee','Hip');
    xlabel('t');
    ylabel([char('w'+i) char('w'+i)]);
    title('Cartesian Coordinates (cm)')
end

figure('Position',[50 50 300 500]);
plot(CoPmeas(:,1),CoPmeas(:,2),'.',[pos(:,1); pos(1,1)],[pos(:,2); pos(1,2)],'linewidth',2);
axis([-50 50 -50 50])
axis equal
grid on
xlabel('CoP x');
ylabel('CoP y');
legend('Measured CoP Trajectory', ...
       'Predicted CoP Trajectory');

figure('Position',[50 50 300 500]);
plot(CoPmeas(:,1),CoPmeas(:,2),'.',[pos(:,1); pos(1,1)],[pos(:,2); pos(1,2)], ...
     xyz(:,1,4),xyz(:,2,4),'g.',xyz(:,1,5),xyz(:,2,5),'r.','linewidth',2);
axis([-70 70 -70 70])
axis equal
grid on
xlabel('CoP x');
ylabel('CoP y');
legend('Measured CoP Trajectory', ...
       'Predicted CoP Trajectory', ...
       'Knee Direct Cinematics', ...
       'Hip Direct Cinematics');

figure
plot(t,theta,'linewidth',2);
grid on
xlabel('t');
ylabel('Joints Position (degrees)');
legend('Foot Pitch', ...
       'Foot Roll', ...
       'Knee');
