close all
%clear all

size=14;
mo=load('traject=-90+50#T=60#M=m41#kp000kd000#t_pos_vel_curr_posTh_velTh#1servo.txt');
mc=load('traject=-90+50#T=60#M=m41#kp010kd040#t_pos_vel_curr_posTh_velTh#1servo.txt');
eo=mo(:,5)-mo(:,2);
ec=mc(:,5)-mc(:,2);

figure(1)
axes('fontsize',size)
plot(mo(:,1),mo(:,5),'b',mo(:,1),mo(:,2),'g',mc(:,1),mc(:,2),'r','linewidth',2);
axis([0 2 -100 60])
grid on
xlabel('time (s)');
ylabel('Position (degrees)')
title('Kp=0.10, Kd=0.40')
legend('Traject requested','Open loop response','Closed loop response','location','SouthEast');
figure(2);
axes('fontsize',size)
plot(mo(:,1),eo(:),'b',mc(:,1),ec(:),'r','linewidth',2);
axis([0 2 -10 40])
grid on
xlabel('time (s)');
ylabel('Error (degrees)')
title('Kp=0.10, Kd=0.40')
legend('Ramp error','Polynomial error','location','SouthWest');


m1=load('rampa-90;+50_M14_kp=0.06kd=0.00#posReq_posEff_contReq.txt');
m2=load('traject=-90+50#T=60#M=m41#kp006kd000#t_pos_vel_curr_posTh_velTh#1servo.txt');
e1=m1(:,2)-m1(:,3);
e2=m2(:,5)-m2(:,2);

figure(3)
axes('fontsize',size)
plot(m2(:,1),m2(:,5),'b',mo(:,1),mo(:,2),'g',m2(:,1),m2(:,2),'r','linewidth',2);
grid on
xlabel('time (s)');
ylabel('Position (degrees)')
title('Kp=0.06, Kd=0.00')
legend('Traject requested','Open loop response','Closed loop response','location','SouthEast');
figure(4);
axes('fontsize',size)
plot(m1(:,1),e1(:),'b',m2(:,1),e2(:),'r','linewidth',2);
grid on
xlabel('time (s)');
ylabel('Error (degrees)')
title('Kp=0.06, Kd=0.00')
legend('Ramp error','Polynomial error','location','SouthWest');


m1=load('rampa-90;+50_M14_kp=0.06kd=0.04#posReq_posEff_contReq.txt');
m2=load('traject=-90+50#T=60#M=m41#kp006kd004#t_pos_vel_curr_posTh_velTh#1servo.txt');
e1=m1(:,2)-m1(:,3);
e2=m2(:,5)-m2(:,2);

figure(5)
axes('fontsize',size)
plot(m2(:,1),m2(:,5),'b',mo(:,1),mo(:,2),'g',m2(:,1),m2(:,2),'r','linewidth',2);
grid on
xlabel('time (s)');
ylabel('Position (degrees)')
title('Kp=0.06, Kd=0.04')
legend('Traject requested','Open loop response','Closed loop response','location','SouthEast');
figure(6);
axes('fontsize',size)
plot(m1(:,1),e1(:),'b',m2(:,1),e2(:),'r','linewidth',2);
grid on
xlabel('time (s)');
ylabel('Error (degrees)')
title('Kp=0.06, Kd=0.04')
legend('Ramp error','Polynomial error','location','SouthWest');


m1=load('rampa-90;+50_M14_kp=0.10kd=0.04#posReq_posEff_contReq.txt');
m2=load('traject=-90+50#T=60#M=m41#kp010kd004#t_pos_vel_curr_posTh_velTh#1servo.txt');
e1=m1(:,2)-m1(:,3);
e2=m2(:,5)-m2(:,2);

figure(7)
axes('fontsize',size)
plot(m2(:,1),m2(:,5),'b',mo(:,1),mo(:,2),'g',m2(:,1),m2(:,2),'r','linewidth',2);
grid on
xlabel('time (s)');
ylabel('Position (degrees)')
title('Kp=0.10, Kd=0.04')
legend('Traject requested','Open loop response','Closed loop response','location','SouthEast');
figure(8);
axes('fontsize',size)
plot(m1(:,1),e1(:),'b',m2(:,1),e2(:),'r','linewidth',2);
axis([0 3 -16 46])
grid on
xlabel('time (s)');
ylabel('Error (degrees)')
title('Kp=0.10, Kd=0.04')
legend('Ramp error','Polynomial error','location','SouthWest');

% figure(7)
% axes('fontsize',size)
% plot(m2(:,1),m2(:,5),'b',m2(:,1),m2(:,2),'m','linewidth',2);
% grid on
% xlabel('time (s)');
% ylabel('Position (degrees)')
% legend('Traject requested','Polynomial response (Kp=0.10, Kd=0.04)','location','SouthEast');

m1=load('rampa-90;+50_M14_kp=0.10kd=0.30#posReq_posEff_contReq.txt');
m2=load('traject=-90+50#T=60#M=m41#kp010kd030#t_pos_vel_curr_posTh_velTh#1servo.txt');
e1=m1(:,2)-m1(:,3);
e2=m2(:,5)-m2(:,2);

figure(9)
axes('fontsize',size)
plot(m2(:,1),m2(:,5),'b',mo(:,1),mo(:,2),'g',m2(:,1),m2(:,2),'r','linewidth',2);
axis([0 2.5 -100 60])
grid on
xlabel('time (s)');
ylabel('Position (degrees)')
title('Kp=0.10, Kd=0.30')
%legend('Traject requested','Open loop response','Closed loop response','location','SouthEast');
figure(10);
axes('fontsize',size)
plot(m1(:,1),e1(:),'b',m2(:,1),e2(:),'r','linewidth',2);
axis([0 2.5 -10 40])
grid on
xlabel('time (s)');
ylabel('Error (degrees)')
title('Kp=0.10, Kd=0.30')
%legend('Ramp error','Polynomial error','location','SouthWest');
