close all
clear all

%_____________________________________________________________
i0=load('Ki=0.10\traject=up_intMaxTorque#T=50#M=m4#ki010kp000kd000#t_pos_vel_curr_posTh_velTh.txt');

p1=load('Ki=0.10\Kp\traject=up_intMaxTorque#T=50#M=m4#ki010kp025kd000#t_pos_vel_curr_posTh_velTh.txt');
p2=load('Ki=0.10\Kp\traject=up_intMaxTorque#T=50#M=m4#ki010kp050kd000#t_pos_vel_curr_posTh_velTh#osc.txt');
p3=load('Ki=0.10\Kp\traject=up_intMaxTorque#T=50#M=m4#ki010kp110kd000#t_pos_vel_curr_posTh_velTh#osc.txt');

d1=load('Ki=0.10\Kd\traject=up_intMaxTorque#T=50#M=m4#ki010kp000kd030#t_pos_vel_curr_posTh_velTh.txt');
d2=load('Ki=0.10\Kd\traject=up_intMaxTorque#T=50#M=m4#ki010kp000kd080#t_pos_vel_curr_posTh_velTh.txt');
d3=load('Ki=0.10\Kd\traject=up_intMaxTorque#T=50#M=m4#ki010kp000kd150#t_pos_vel_curr_posTh_velTh#osc.txt');

%Tempo
timei0=i0(:,1);
timep1=p1(:,1);             timed1=d1(:,1);
timep2=p2(:,1);             timed2=d2(:,1);
timep3=p3(:,1);             timed3=d3(:,1);

% Trajectória prevista
predictedi0=i0(:,5);
predictedp1=p1(:,5);        predictedd1=d1(:,5);
predictedp2=p2(:,5);        predictedd2=d2(:,5);
predictedp3=p3(:,5);        predictedd3=d3(:,5);

% Trajectória real
reali0=i0(:,2);
realp1=p1(:,2);             reald1=d1(:,2);
realp2=p2(:,2);             reald2=d2(:,2);
realp3=p3(:,2);             reald3=d3(:,2);

% Erro
errori0=predictedi0-reali0;
errorp1=predictedp1-realp1; errord1=predictedd1-reald1;
errorp2=predictedp2-realp2; errord2=predictedd2-reald2;
errorp3=predictedp3-realp3; errord3=predictedd3-reald3;

figure('name','(Massa=675g) Variando Kp com Ki=0.10');
axes('fontsize',14);
plot(timei0,predictedi0,'k',timei0,reali0,'r',timep1,realp1,'m',timep2,realp2,'g',timep3,realp3,'b','linewidth',2);
xlabel('time (s)');
ylabel('position (degrees)');
legend('Expected','Kp=0.00','Kp=0.25','Kp=0.50','Kp=1.10','location','southeast');

figure('name','(Massa=675g) Variando Kp com Ki=0.10');
axes('fontsize',14);
plot(timei0,errori0,timep1,errorp1,timep2,errorp2,timep3,errorp3,'linewidth',2);
grid on
xlabel('time (s)');
ylabel('error (degrees)');

figure('name','(Massa=675g) Variando Kd com Ki=0.10');
axes('fontsize',14);
plot(timei0,predictedi0,'k',timei0,reali0,timed1,reald1,timed2,reald2,timed3,reald3,'linewidth',2);
xlabel('time (s)');
ylabel('position (degrees)');
legend('Expected','Kd=0.00','Kd=0.30','Kd=0.80','Kd=1.50','location','southeast');

figure('name','(Massa=675g) Variando Kd com Ki=0.10');
axes('fontsize',14);
plot(timei0,errori0,timed1,errord1,timed2,errord2,timed3,errord3,'linewidth',2);
grid on
xlabel('time (s)');
ylabel('error (degrees)');
