close all
clear all

size=14;

mo=load('traject=-90+00#T=050#M=m421#kp000kd000#t_pos_vel_curr_posTh_velTh.txt');
mc=load('traject=-90+00#T=050#M=m421#kp020kd100#t_pos_vel_curr_posTh_velTh.txt');
eo=mo(:,5)-mo(:,2);
ec=mc(:,5)-mc(:,2);
N=min(length(mo),length(mc));

figure(1)
axes('fontsize',size)
plot(mo(1:N,1),mo(1:N,5),'b',mo(1:N,1),mo(1:N,2),'m',mc(1:N,1),mc(1:N,2),'r','linewidth',2);
grid on
xlabel('time (s)','fontsize',size);
ylabel('Position (degrees)','fontsize',size)
legend('Traject requested','Open loop response','Closed loop response','location','SouthEast');

figure(2);
axes('fontsize',size)
plot(mo(1:N,1),eo(1:N),'m',mc(1:N,1),ec(1:N),'r','linewidth',2);
grid on
xlabel('time (s)','fontsize',size);
ylabel('Error (degrees)','fontsize',size)
legend('Open loop response','Closed loop response','location','SouthWest');


mo=load('traject=-70+70#T=050#M=m42#kp000kd000#t_pos_vel_curr_posTh_velTh.txt');
mc=load('traject=-70+70#T=050#M=m42#kp006kd020#t_pos_vel_curr_posTh_velTh.txt');
eo=mo(:,5)-mo(:,2);
ec=mc(:,5)-mc(:,2);
N=min(length(mo),length(mc));

figure(3)
axes('fontsize',size)
plot(mo(1:N,1),mo(1:N,5),'b',mo(1:N,1),mo(1:N,2),'m',mc(1:N,1),mc(1:N,2),'r','linewidth',2);
grid on
xlabel('time (s)','fontsize',size);
ylabel('Position (degrees)','fontsize',size)
legend('Traject requested','Open loop response','Closed loop response','location','SouthEast');

figure(4);
axes('fontsize',size)
plot(mo(1:N,1),eo(1:N),'m',mc(1:N,1),ec(1:N),'r','linewidth',2);
grid on
xlabel('time (s)','fontsize',size);
ylabel('Error (degrees)','fontsize',size)
legend('Open loop response','Closed loop response','location','SouthWest');




mo=load('traject=-70+70#T=075#M=m42#kp000kd000#t_pos_vel_curr_posTh_velTh.txt');
mc=load('traject=-70+70#T=075#M=m42#kp015kd030#t_pos_vel_curr_posTh_velTh.txt');
eo=mo(:,5)-mo(:,2);
ec=mc(:,5)-mc(:,2);
N=min(length(mo),length(mc));

figure(5)
axes('fontsize',size)
plot(mo(1:N,1),mo(1:N,5),'b',mo(1:N,1),mo(1:N,2),'m',mc(1:N,1),mc(1:N,2),'r','linewidth',2);
axis([0 1.9 -80 80])
grid on
xlabel('time (s)','fontsize',size);
ylabel('Position (degrees)','fontsize',size)
legend('Traject requested','Open loop response','Closed loop response','location','SouthEast');

figure(6);
axes('fontsize',size)
plot(mo(1:N,1),eo(1:N),'m',mc(1:N,1),ec(1:N),'r','linewidth',2);
axis([0 1.9 -5 35])
grid on
xlabel('time (s)','fontsize',size);
ylabel('Error (degrees)','fontsize',size)
legend('Open loop response','Closed loop response','location','SouthWest');
