% Análise do trajecto entre binários gravíticos intermédios
% passando pelo binário nulo! Inicialmente o servo sobe até ao ponto de
% binário nulo, e a partir daí começa a descer até a posição final.

close all

figureName='Trajecto entre binários intermédios passando pelo valor nulo (Subida->Descida)';

curr_norm=46;

m2o=load('traject=upDown_intMinTorque#T=50#M=m2#ki000kp000kd000#t_pos_vel_curr_posTh_velTh.txt');
m2=load('traject=upDown_intMinTorque#T=50#M=m2#ki025kp020kd000#t_pos_vel_curr_posTh_velTh.txt');
m2d=load('traject=upDown_intMinTorque#T=50#M=m2#ki025kp020kd020#t_pos_vel_curr_posTh_velTh.txt');

m4o=load('traject=upDown_intMinTorque#T=50#M=m4#ki000kp000kd000#t_pos_vel_curr_posTh_velTh.txt');
m4=load('traject=upDown_intMinTorque#T=50#M=m4#ki025kp020kd000#t_pos_vel_curr_posTh_velTh.txt');
m4d=load('traject=upDown_intMinTorque#T=50#M=m4#ki025kp020kd020#t_pos_vel_curr_posTh_velTh.txt');

m42o=load('traject=upDown_intMinTorque#T=50#M=m42#ki000kp000kd000#t_pos_vel_curr_posTh_velTh.txt');
m42=load('traject=upDown_intMinTorque#T=50#M=m42#ki020kp030kd000#t_pos_vel_curr_posTh_velTh.txt');
m42d=load('traject=upDown_intMinTorque#T=50#M=m42#ki020kp030kd140#t_pos_vel_curr_posTh_velTh.txt');

figure('name',[figureName,': Período do trajecto=1s']);
subplot(3,3,1);
plot(m2(:,1),m2(:,5),'k:',m2o(:,1),m2o(:,2),'g',m2(:,1),m2(:,2),'b',m2d(:,1),m2d(:,2),'r');
xlabel('Trajectória realizada');
title('Massa=463g (Ki=0.25, Kp=0.20, Kd=0.20)');
legend('Trajectória','Malha aberta','PI Controller','PID Controller','location','northeast');
subplot(3,3,4);
plot(m2o(:,1),m2o(:,5)-m2o(:,2),'g',m2(:,1),m2(:,5)-m2(:,2),'b',m2d(:,1),m2d(:,5)-m2d(:,2),'r');
xlabel('Sinal de erro');
subplot(3,3,7);
plot(m2(:,1),abs(m2(:,6))/max(abs(m2(:,6))),'k:',m2o(:,1),m2o(:,4)/curr_norm,'g',m2(:,1),m2(:,4)/curr_norm,'b',m2d(:,1),m2d(:,4)/curr_norm,'r');
xlabel('Corrente vs Velocidade prevista');
legend('Velocidade','location','northeast');

subplot(3,3,2);
plot(m4(:,1),m4(:,5),'k:',m4o(:,1),m4o(:,2),'g',m4(:,1),m4(:,2),'b',m4d(:,1),m4d(:,2),'r');
xlabel('Trajectória realizada');
title('Massa=675g (Ki=0.25, Kp=0.20, Kd=0.20)');
subplot(3,3,5);
plot(m4o(:,1),m4o(:,5)-m4o(:,2),'g',m4(:,1),m4(:,5)-m4(:,2),'b',m4d(:,1),m4d(:,5)-m4d(:,2),'r');
xlabel('Sinal de erro');
subplot(3,3,8);
plot(m4(:,1),abs(m4(:,6))/max(abs(m4(:,6))),'k:',m4o(:,1),m4o(:,4)/curr_norm,'g',m4(:,1),m4(:,4)/curr_norm,'b',m4d(:,1),m4d(:,4)/curr_norm,'r');
xlabel('Corrente vs Velocidade prevista');

subplot(3,3,3);
plot(m42(:,1),m42(:,5),'k:',m42o(:,1),m42o(:,2),'g',m42(:,1),m42(:,2),'b',m42d(:,1),m42d(:,2),'r');
xlabel('Trajectória realizada');
title('Massa=1129g (Ki=0.20, Kp=0.30, Kd=1.40)');
subplot(3,3,6);
plot(m42o(:,1),m42o(:,5)-m42o(:,2),'g',m42(:,1),m42(:,5)-m42(:,2),'b',m42d(:,1),m42d(:,5)-m42d(:,2),'r');
xlabel('Sinal de erro');
subplot(3,3,9);
plot(m42(:,1),abs(m42(:,6))/max(abs(m42(:,6))),'k:',m42o(:,1),m42o(:,4)/curr_norm,'g',m42(:,1),m42(:,4)/curr_norm,'b',m42d(:,1),m42d(:,4)/curr_norm,'r');
xlabel('Corrente vs Velocidade prevista');

%__________________________________________________________________________

m2o=load('traject=upDown_intMinTorque#T=75#M=m2#ki000kp000kd000#t_pos_vel_curr_posTh_velTh.txt');
m2=load('traject=upDown_intMinTorque#T=75#M=m2#ki020kp000kd000#t_pos_vel_curr_posTh_velTh.txt');
m2d=load('traject=upDown_intMinTorque#T=75#M=m2#ki020kp000kd020#t_pos_vel_curr_posTh_velTh.txt');

m4o=load('traject=upDown_intMinTorque#T=75#M=m4#ki000kp000kd000#t_pos_vel_curr_posTh_velTh.txt');
m4=load('traject=upDown_intMinTorque#T=75#M=m4#ki018kp000kd000#t_pos_vel_curr_posTh_velTh.txt');
m4d=load('traject=upDown_intMinTorque#T=75#M=m4#ki018kp000kd020#t_pos_vel_curr_posTh_velTh.txt');

m42o=load('traject=upDown_intMinTorque#T=75#M=m42#ki000kp000kd000#t_pos_vel_curr_posTh_velTh.txt');
m42=load('traject=upDown_intMinTorque#T=75#M=m42#ki015kp000kd000#t_pos_vel_curr_posTh_velTh.txt');
m42d=load('traject=upDown_intMinTorque#T=75#M=m42#ki015kp000kd010#t_pos_vel_curr_posTh_velTh.txt');

figure('name',[figureName,': Período do trajecto=1.5s']);
subplot(3,3,1);
plot(m2(:,1),m2(:,5),'k:',m2o(:,1),m2o(:,2),'g',m2(:,1),m2(:,2),'b',m2d(:,1),m2d(:,2),'r');
xlabel('Trajectória realizada');
title('Massa=463g (Ki=0.20, Kp=0.00, Kd=0.20)');
legend('Trajectória','Malha aberta','PI Controller','PID Controller','location','northeast');
subplot(3,3,4);
plot(m2o(:,1),m2o(:,5)-m2o(:,2),'g',m2(:,1),m2(:,5)-m2(:,2),'b',m2d(:,1),m2d(:,5)-m2d(:,2),'r');
xlabel('Sinal de erro');
subplot(3,3,7);
plot(m2(:,1),abs(m2(:,6))/max(abs(m2(:,6))),'k:',m2o(:,1),m2o(:,4)/curr_norm,'g',m2(:,1),m2(:,4)/curr_norm,'b',m2d(:,1),m2d(:,4)/curr_norm,'r');
xlabel('Corrente vs Velocidade prevista');
legend('Velocidade','location','northeast');

subplot(3,3,2);
plot(m4(:,1),m4(:,5),'k:',m4o(:,1),m4o(:,2),'g',m4(:,1),m4(:,2),'b',m4d(:,1),m4d(:,2),'r');
xlabel('Trajectória realizada');
title('Massa=675g (Ki=0.18, Kp=0.00, Kd=0.20)');
subplot(3,3,5);
plot(m4o(:,1),m4o(:,5)-m4o(:,2),'g',m4(:,1),m4(:,5)-m4(:,2),'b',m4d(:,1),m4d(:,5)-m4d(:,2),'r');
xlabel('Sinal de erro');
subplot(3,3,8);
plot(m4(:,1),abs(m4(:,6))/max(abs(m4(:,6))),'k:',m4o(:,1),m4o(:,4)/curr_norm,'g',m4(:,1),m4(:,4)/curr_norm,'b',m4d(:,1),m4d(:,4)/curr_norm,'r');
xlabel('Corrente vs Velocidade prevista');

subplot(3,3,3);
plot(m42(:,1),m42(:,5),'k:',m42o(:,1),m42o(:,2),'g',m42(:,1),m42(:,2),'b',m42d(:,1),m42d(:,2),'r');
xlabel('Trajectória realizada');
title('Massa=1129g (Ki=0.15, Kp=0.00, Kd=0.10)');
subplot(3,3,6);
plot(m42o(:,1),m42o(:,5)-m42o(:,2),'g',m42(:,1),m42(:,5)-m42(:,2),'b',m42d(:,1),m42d(:,5)-m42d(:,2),'r');
xlabel('Sinal de erro');
subplot(3,3,9);
plot(m42(:,1),abs(m42(:,6))/max(abs(m42(:,6))),'k:',m42o(:,1),m42o(:,4)/curr_norm,'g',m42(:,1),m42(:,4)/curr_norm,'b',m42d(:,1),m42d(:,4)/curr_norm,'r');
xlabel('Corrente vs Velocidade prevista');
