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

clear all
close all

a1=load('traject=up_intMaxTorque#T=50#M=m4#ki005kp000kd000#t_pos_vel_curr_posTh_velTh.txt');
a2=load('traject=up_intMaxTorque#T=50#M=m4#ki005kp030kd000#t_pos_vel_curr_posTh_velTh.txt');
a3=load('traject=up_intMaxTorque#T=50#M=m4#ki005kp080kd000#t_pos_vel_curr_posTh_velTh.txt');
a4=load('traject=up_intMaxTorque#T=50#M=m4#ki005kp150kd000#t_pos_vel_curr_posTh_velTh.txt');

%_____________________________________________________________
figure('name','(Massa=675g) Variando Kp com Ki=0.05');
time1=a1(:,1);
time2=a2(:,1);
time3=a3(:,1);
time4=a4(:,1);

predicted1=a1(:,5);
predicted2=a2(:,5);
predicted3=a3(:,5);
predicted4=a4(:,5);

real1=a1(:,2);
real2=a2(:,2);
real3=a3(:,2);
real4=a4(:,2);

error1=predicted1-real1;
error2=predicted2-real2;
error3=predicted3-real3;
error4=predicted4-real4;


subplot(2,1,1);
plot(time1,predicted1,time1,real1,time2,real2,time3,real3,time4,real4);
xlabel('time');
ylabel('position');
legend('Expected','Kp=0.00','Kp=0.10','Kp=0.30','Kp=1.00','location','southeast');
subplot(2,1,2);
plot(time1,error1,time2,error2,time3,error3,time4,error4);
grid on
xlabel('time');
ylabel('error');
