close all
clear all

size=14;

mc1=load('rampa-90;+50_M14_kp=0.06kd=0.00#posReq_posEff_contReq.txt');
mc2=load('rampa-90;+50_M14_kp=0.06kd=0.04#posReq_posEff_contReq.txt');
e1=mc1(:,2)-mc1(:,3);
e2=mc2(:,2)-mc2(:,3);
N=floor(length(mc1)*4/5);
figure(5);
axes('fontsize',size)
plot(mc1(1:N,1),mc1(1:N,2),'b',mc1(1:N,1),mc1(1:N,3),'m',mc1(1:N,1),mc2(1:N,4),'g','linewidth',2);
grid on
xlabel('time (s)','fontsize',size);
ylabel('Position (degrees)','fontsize',size)
legend('Ramp requested','Closed loop response','Output control signal','location','SouthEast');

figure(6)
axes('fontsize',size)
plot(mc1(1:N,1),mc1(1:N,2),'b',mc1(1:N,1),mc1(1:N,3),'m',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','Kp=0.06, Kd=0.00','Kp=0.06, Kd=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),'r','linewidth',2);
grid on
xlabel('time (s)','fontsize',size);
ylabel('Position (degrees)','fontsize',size)
legend('Kp=0.06, Kd=0.00','Kp=0.06, Kd=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),'b',mc1(1:N,1),mc1(1:N,3),'m',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),'m',mc2(1:N,1),e2(1:N),'r','linewidth',2);
grid on
xlabel('time (s)','fontsize',size);
ylabel('Position (degrees)','fontsize',size)
legend('Kp=0.10, Kd=0.04','Kp=0.10, Kd=0.30','location','NorthEast');


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

figure(11);
axes('fontsize',size)
plot(mc1(1:N,1),e1(1:N),'m',mc2(1:N,1),e2(1:N),'r','linewidth',2);
grid on
xlabel('time (s)','fontsize',size);
ylabel('Position (degrees)','fontsize',size)
legend('Kp=0.30, Kd=0.30','Kp=0.30, Kd=1.50','location','NorthEast');
