close all
clear all

size=14;

mc1=load('rampa-90;+50_M14_kp=0.06kd=0.04#posReq_posEff_contReq.txt');
mc2=load('rampa-90;+50_M14_kp=0.10kd=0.04#posReq_posEff_contReq.txt');
e1=mc1(:,2)-mc1(:,3);
e2=mc2(:,2)-mc2(:,3);
N=floor(length(mc1)*4/5);

figure(6)
axes('fontsize',size)
plot(mc1(1:N,1),mc1(1:N,2),'k',mc1(1:N,1),mc1(1:N,3),'m',mc2(1:N,1),mc2(1:N,3),'b','linewidth',2);
grid on
xlabel('time (s)','fontsize',size);
ylabel('Position (degrees)','fontsize',size)
legend('Ramp requested','Ki=0.06, Kp=0.04','Ki=0.10, Kp=0.04','location','SouthEast');

figure(7);
axes('fontsize',size)
plot(mc1(1:N,1),e1(1:N),'m',mc2(1:N,1),e2(1:N),'b','linewidth',2);
axis([0 4 -10 52])
grid on
xlabel('time (s)','fontsize',size);
ylabel('Error (degrees)','fontsize',size)
legend('Ki=0.06, Kp=0.04','Ki=0.10, Kp=0.04','location','NorthEast');


mc1=load('rampa-90;+50_M14_kp=0.10kd=0.04#posReq_posEff_contReq.txt');
mc2=load('rampa-90;+50_M14_kp=0.10kd=0.30#posReq_posEff_contReq.txt');
e1=mc1(:,2)-mc1(:,3);
e2=mc2(:,2)-mc2(:,3);
N=floor(length(mc1)*4/5);
figure(8)
axes('fontsize',size)
plot(mc1(1:N,1),mc1(1:N,2),'k',mc1(1:N,1),mc1(1:N,3),'b',mc2(1:N,1),mc2(1:N,3),'r','linewidth',2);
grid on
xlabel('time (s)','fontsize',size);
ylabel('Position (degrees)','fontsize',size)
legend('Ramp requested','Ki=0.10, Kp=0.04','Ki=0.10, Kp=0.30','location','SouthEast');

figure(9);
axes('fontsize',size)
plot(mc1(1:N,1),e1(1:N),'b',mc2(1:N,1),e2(1:N),'r','linewidth',2);
axis([0 4 -10 52])
grid on
xlabel('time (s)','fontsize',size);
ylabel('Error (degrees)','fontsize',size)
legend('Ki=0.10, Kp=0.04','Ki=0.10, Kp=0.30','location','NorthEast');

mc1=load('rampa-90;+50_M14_kp=0.06kd=0.04#posReq_posEff_contReq.txt');
mc2=load('rampa-90;+50_M14_kp=0.10kd=0.04#posReq_posEff_contReq.txt');
mc3=load('rampa-90;+50_M14_kp=0.10kd=0.30#posReq_posEff_contReq.txt');
e1=mc1(:,2)-mc1(:,3);
e2=mc2(:,2)-mc2(:,3);
e3=mc3(:,2)-mc3(:,3);
N=floor(length(mc1)*4/5);
figure(10);
axes('fontsize',size)
plot(mc1(1:N,1),mc1(1:N,2),'k',mc1(1:N,1),mc1(1:N,3),'g',mc2(1:N,1),mc2(1:N,3),'r',mc3(1:N,1),mc3(1:N,3),'b','linewidth',2);
grid on
xlabel('time (s)','fontsize',size);
ylabel('Position (degrees)','fontsize',size)
legend('Ramp requested','Ki=0.06, Kp=0.04','Ki=0.10, Kp=0.04','Ki=0.10, Kp=0.30','location','SouthEast');

figure(11);
axes('fontsize',size)
plot(mc1(1:N,1),e1(1:N),'g',mc2(1:N,1),e2(1:N),'r',mc3(1:N,1),e3(1:N),'b','linewidth',2);
axis([0 4 -10 52])
grid on
xlabel('time (s)','fontsize',size);
ylabel('Error (degrees)','fontsize',size)
