clear all
close all

title4= 'Resposta à Rampa: -45º=>+45º, timeStep=100ms, positionStep=5º, Carga=M1+M4=933g';
files4=['rampaM14nocontrol_posReq_posEff_kt0#-45;+45_dt100.txt    ',
        'rampaM14kp=002_posReq_posEff_controlReq#-45;+45_dt100.txt',
        'rampaM14kp=004_posReq_posEff_controlReq#-45;+45_dt100.txt',
        'rampaM14kp=007_posReq_posEff_controlReq#-45;+45_dt100.txt',
        'rampaM14kp=010_posReq_posEff_controlReq#-45;+45_dt100.txt',
        'rampaM14kp=015_posReq_posEff_controlReq#-45;+45_dt100.txt',
        'rampaM14kp=020_posReq_posEff_controlReq#-45;+45_dt100.txt',
        'rampaM14kp=030_posReq_posEff_controlReq#-45;+45_dt100.txt'];
label4=['Sem controlo',
        'Kp=0.02     ',
        'Kp=0.04     ',
        'Kp=0.07     ',
        'Kp=0.10     ',
        'Kp=0.15     ',
        'Kp=0.20     ',
        'Kp=0.30     '];

legenda=['Requested trajectory';
         'Open loop response  ';
         'Closed loop response';
         'Controller output   '];

for i=1:size(files4,1)
    clear array;
    array=load(files4(i,:));
    
    f=figure;
    set(f,'name',label4(i,:));
    axes('fontsize',14);
    
    if (i==1)
        array0=array;
        plot(array0(:,1),array0(:,2),'k',array0(:,1),array0(:,3),'b','linewidth',2);
    else
        plot(array(:,1),array(:,2),'k',array0(:,1),array0(:,3),'b',...
             array(:,1),array(:,3),'r',array(:,1),array(:,4),'m','linewidth',2);
%         plot(array(:,1),array(:,2),'g',array0(:,1),array0(:,3),'b',...
%              array(:,1),array(:,3),'r',array(:,1),array(:,4),'y');
    end
    
    axis([0,5,-60,60]);
    xlabel('time (s)');
    ylabel('Position (degrees)');
    
    legend(legenda,'location','southeast');

    grid on
end


a0=load('rampaM14nocontrol_posReq_posEff_kt0#-45;+45_dt100.txt');
a1=load('rampaM14kp=002_posReq_posEff_controlReq#-45;+45_dt100.txt');
a2=load('rampaM14kp=007_posReq_posEff_controlReq#-45;+45_dt100.txt');
a3=load('rampaM14kp=015_posReq_posEff_controlReq#-45;+45_dt100.txt');
a4=load('rampaM14kp=030_posReq_posEff_controlReq#-45;+45_dt100.txt');

figure;
axes('fontsize',14);

plot(a0(:,1),a0(:,2),'k',a0(:,1),a0(:,3),'g', ...
     a1(:,1),a1(:,3),'c',a2(:,1),a2(:,3),'b', ...
     a3(:,1),a3(:,3),'m',a4(:,1),a4(:,3),'r','linewidth',2);
axis([0,5,-60,60]);
grid on

xlabel('time (s)')
ylabel('position (degrees)')

legend('Requested trajectory', ...
       'Open loop response', ...
       'K_i=0.02','K_i=0.07','K_i=0.15','K_i=0.30');