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

a0 =load('traject=up_intMaxTorque#T=50#M=m4#ki020kp070kd030#t_pos_vel_curr_posTh_velTh#THIS.txt');
a1 =load('traject=up_intMaxTorque#T=50#M=m4#ki030kp080kd005#t_pos_vel_curr_posTh_velTh.txt');
a2 =load('traject=up_intMaxTorque#T=50#M=m4#ki035kp125kd013#t_pos_vel_curr_posTh_velTh.txt');
a3a=load('traject=up_intMaxTorque#T=50#M=m4#ki039kp146kd015#t_pos_vel_curr_posTh_velTh.txt');
a3b=load('traject=up_intMaxTorque#T=50#M=m4#ki039kp146kd015#t_pos_vel_curr_posTh_velTh#2ndExp.txt');
a4 =load('traject=up_intMaxTorque#T=50#M=m4#ki041kp149kd013#t_pos_vel_curr_posTh_velTh#overshoot.txt');

close all
%_____________________________________________________________
figure('name','(Massa=675g) Primeiro tunning: Ki=0.20, Kp=0.70, Kd=0.30');
time=a0(:,1);
predicted=a0(:,5);
real=a0(:,2);
error=predicted-real;

subplot(2,1,1);
plot(time,predicted,time,real);
xlabel('time');
ylabel('position');
subplot(2,1,2);
plot(time,error);
axis([0 3 -3 14]);
grid on
xlabel('time');
ylabel('error');

%_____________________________________________________________
figure('name','(Massa=675g) Segundo tunning: Ki=0.30, Kp=0.80, Kd=0.05');
time=a1(:,1);
predicted=a1(:,5);
real=a1(:,2);
error=predicted-real;

subplot(2,1,1);
plot(time,predicted,time,real);
xlabel('time');
ylabel('position');
subplot(2,1,2);
plot(time,error);
grid on
axis([0 3 -3 14]);
xlabel('time');
ylabel('error');

%_____________________________________________________________
figure('name','(Massa=675g) Terceiro tunning: Ki=0.35, Kp=1.25, Kd=0.13');
time=a2(:,1);
predicted=a2(:,5);
real=a2(:,2);
error=predicted-real;

subplot(2,1,1);
plot(time,predicted,time,real);
xlabel('time');
ylabel('position');
subplot(2,1,2);
plot(time,error);
axis([0 3 -3 14]);
grid on
xlabel('time');
ylabel('error');

%_____________________________________________________________
figure('name','(Massa=675g) Terceiro tunning: Ki=0.39, Kp=1.46, Kd=0.15 => Comparação entre duas experiencias iguais');
timea=a3a(:,1);
predicteda=a3a(:,5);
reala=a3a(:,2);
timeb=a3b(:,1);
predictedb=a3b(:,5);
realb=a3b(:,2);

errora=predicteda-reala;
errorb=predictedb-realb;

subplot(2,2,1);
plot(timea,predicteda,timea,reala);
xlabel('time');
ylabel('position');
title('Primeira vez')
subplot(2,2,2);
plot(timeb,predictedb,timeb,realb);
xlabel('time');
ylabel('position');
title('Segunda vez')
subplot(2,2,3);
plot(timea,errora);
axis([0 3 -3 14]);
grid on
xlabel('time');
ylabel('error');
subplot(2,2,4);
plot(timeb,errorb);
axis([0 3 -3 14]);
grid on
xlabel('time');
ylabel('error');

%_____________________________________________________________
figure('name','(Massa=675g) Quarto tunning: Ki=0.41, Kp=1.49, Kd=0.13 (Falhado)');
time=a4(:,1);
predicted=a4(:,5);
real=a4(:,2);
error=predicted-real;
curr=a4(:,4);

subplot(3,1,1);
plot(time,predicted,time,real);
xlabel('time');
ylabel('position');
subplot(3,1,2);
plot(time,error);
axis([0 3 -3 14]);
grid on
xlabel('time');
ylabel('error');
subplot(3,1,3);
plot(time,curr/52);
axis([0 3 0 1]);
grid on
xlabel('time');
ylabel('current');
