% 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.

curr_norm=46;

figureName='Massa=1129g: trajecto entre binários intermédios passando pelo valor mínimo';

m42o=load('traject=upDown_intMinTorque#T=50#M=m42#ki000kp000kd000#t_pos_vel_curr_posTh_velTh.txt');
m42c=load('traject=upDown_intMinTorque#T=50#M=m42#ki020kp030kd000#t_pos_vel_curr_posTh_velTh.txt');
m42d1=load('traject=upDown_intMinTorque#T=50#M=m42#ki020kp030kd100#t_pos_vel_curr_posTh_velTh.txt');
m42d2=load('traject=upDown_intMinTorque#T=50#M=m42#ki020kp030kd140#t_pos_vel_curr_posTh_velTh.txt');
m42d3=load('traject=upDown_intMinTorque#T=50#M=m42#ki020kp030kd160#t_pos_vel_curr_posTh_velTh.txt');

eo=m42o(:,5)-m42o(:,2);
ec=m42c(:,5)-m42c(:,2);
ed1=m42d1(:,5)-m42d1(:,2);
ed2=m42d2(:,5)-m42d2(:,2);
ed3=m42d3(:,5)-m42d3(:,2);

close all

figure('name',['(' figureName ') Comparação entre 2 tipos de compensadores']);
subplot(2,1,1);
plot(m42o(:,1),m42o(:,5),'k:',m42o(:,1),m42o(:,2),'g',m42c(:,1),m42c(:,2),'b',m42d2(:,1),m42d2(:,2),'r');
xlabel('Trajectória realizada');
legend('Trajectória prevista','Malha aberta','Compensação PI (Ki=0.20,Kp=30)','Compensação PID (Ki=0.20,Kp=30,Kd=1.40)','location','northeast');
subplot(2,1,2);
plot(m42o(:,1),eo,'g',m42c(:,1),ec,'b',m42d2(:,1),ed2,'r');
xlabel('Sinal de erro');
legend('Malha aberta','Compensação PI (Ki=0.20,Kp=30)','Compensação PID (Ki=0.20,Kp=30,Kd=1.40)','location','southeast');

figure('name',['(' figureName ') Compensador PID: comparando diferentes Kd']);
subplot(2,1,1);
plot(m42o(:,1),m42o(:,5),'k:',m42d1(:,1),m42d1(:,2),'g',m42d2(:,1),m42d2(:,2),'b',m42d3(:,1),m42d3(:,2),'r');
xlabel('Trajectória realizada');
legend('Trajectória prevista','Ki=0.20,Kp=30,Kd=1.00','Ki=0.20,Kp=30,Kd=1.40','Ki=0.20,Kp=30,Kd=1.60','location','northeast');
subplot(2,1,2);
plot(m42d1(:,1),ed1,'g',m42d2(:,1),ed2,'b',m42d3(:,1),ed3,'r');
xlabel('Sinal de erro');
legend('Ki=0.20,Kp=30,Kd=1.00','Ki=0.20,Kp=30,Kd=1.40','Ki=0.20,Kp=30,Kd=1.60','location','southeast');

figure('name',['(' figureName ') Comparação dos níveis de corrente']);
subplot(2,1,1);
plot(m42o(:,1),abs(m42o(:,6))/max(abs(m42o(:,6))),'k:',m42o(:,1),m42o(:,4)/curr_norm,'g',m42c(:,1),m42c(:,4)/curr_norm,'b',m42d2(:,1),m42d2(:,4)/curr_norm,'r');
xlabel('Corrente');
legend('Velocidade prevista','Malha aberta','Compensação PI (Ki=0.20,Kp=30)','Compensação PID (Ki=0.20,Kp=30,Kd=1.40)','location','northeast');
subplot(2,1,2);
plot(m42o(:,1),abs(m42o(:,6))/max(abs(m42o(:,6))),'k:',m42d1(:,1),m42d1(:,4)/curr_norm,'g',m42d2(:,1),m42d2(:,4)/curr_norm,'b',m42d3(:,1),m42d3(:,4)/curr_norm,'r');
xlabel('Corrente');
legend('Velocidade prevista','Ki=0.20,Kp=30,Kd=1.00','Ki=0.20,Kp=30,Kd=1.40','Ki=0.20,Kp=30,Kd=1.60','location','northeast');
