% 4 experiências de afinação dos parametros óptimos
% A primeira figura apresenta a primeira tentativa

%close all
clear all

files=['the best\traject=up_intMaxTorque#T=50#M=m4#ki020kp070kd030#t_pos_vel_curr_posTh_velTh#THIS.txt     ';
       'the best\traject=up_intMaxTorque#T=50#M=m4#ki030kp080kd005#t_pos_vel_curr_posTh_velTh.txt          ';
       'the best\traject=up_intMaxTorque#T=50#M=m4#ki035kp125kd013#t_pos_vel_curr_posTh_velTh.txt          ';
       'the best\traject=up_intMaxTorque#T=50#M=m4#ki039kp146kd015#t_pos_vel_curr_posTh_velTh.txt          ';
       'the best\traject=up_intMaxTorque#T=50#M=m4#ki039kp146kd015#t_pos_vel_curr_posTh_velTh#2ndExp.txt   ';
       'the best\traject=up_intMaxTorque#T=50#M=m4#ki041kp149kd013#t_pos_vel_curr_posTh_velTh#overshoot.txt'];

labels=['(Massa=675g) Primeiro tunning: Ki=0.20, Kp=0.70, Kd=0.30        ';
        '(Massa=675g) Segundo tunning:  Ki=0.30, Kp=0.80, Kd=0.05        ';
        '(Massa=675g) Terceiro tunning: Ki=0.35, Kp=1.25, Kd=0.13        ';
        '(Massa=675g) Quarto tunning: Ki=0.39, Kp=1.46, Kd=0.15 => Exp1  ';
        '(Massa=675g) Quarto tunning: Ki=0.39, Kp=1.46, Kd=0.15 => Exp2  ';
        '(Massa=675g) Quinto tunning: Ki=0.41, Kp=1.49, Kd=0.13 (Falhado)'];

for i=1:6
    
    a=load(files(i,:));
    
    time=a(:,1);
    pred=a(:,5);
    real=a(:,2);

    time=time(1:floor(end*2/3));
    pred=pred(1:floor(end*2/3));
    real=real(1:floor(end*2/3));

    error=pred-real;
    
    figure('name',labels(i,:));

    subplot(2,1,1);
    plot(time,pred,'k',time,real,'b','linewidth',2);
    grid on
    xlabel('time (s)','fontsize',14);
    ylabel('position (degrees)','fontsize',14);

    subplot(2,1,2);
    plot(time,error,'b','linewidth',2);
    axis([0 2 -5 15])
    grid on
    xlabel('time (s)','fontsize',14);
    ylabel('error (degrees)','fontsize',14);
    
    legend(labels(i,:));
end

a1=load('the best\traject=up_intMaxTorque#T=50#M=m4#ki020kp070kd030#t_pos_vel_curr_posTh_velTh#THIS.txt');
a2=load('the best\traject=up_intMaxTorque#T=50#M=m4#ki035kp125kd013#t_pos_vel_curr_posTh_velTh.txt');

time1=a1(:,1);
pred1=a1(:,5);
real1=a1(:,2);
time2=a2(:,1);
pred2=a2(:,5);
real2=a2(:,2);

time1=time1(1:floor(end*2/3));
pred1=pred1(1:floor(end*2/3));
real1=real1(1:floor(end*2/3));
time2=time2(1:floor(end*2/3));
pred2=pred2(1:floor(end*2/3));
real2=real2(1:floor(end*2/3));

error1=pred1-real1;
error2=pred2-real2;

figure;

figure('name','(675g) Comparação entre dois tunnings');

subplot(2,1,1);
plot(time1,pred1,'k',time1,real1,'b',time2,real2,'r','linewidth',2);
grid on
xlabel('time (s)','fontsize',14);
ylabel('position (degrees)','fontsize',14);

subplot(2,1,2);
plot(time1,error1,'b',time2,error2,'r','linewidth',2);
grid on
xlabel('time (s)','fontsize',14);
ylabel('error (degrees)','fontsize',14);
legend('Tunning 1: Ki=0.20, Kp=0.70, Kd=0.30', ...
       'Tunning 2: Ki=0.35, Kp=1.25, Kd=0.13');