% Análise dos efeitos de variação dos parametros do controlador
%
% Variação de Kp:
% Variando Kp observa-se que o tempo de atraso (0=>50%) aumenta com o
% aumento de Kp, mas o tempo de estabelecimento é prejudicado. Observa-se
% na resposta ao polinómio um ponto comum a todos os Kp's, em que antes
% de ser atingido o atraso é melhorado, mas depois de ultrapassado é
% deteriorado. É fundamental assim um compromisso com o valor de Kp.
% Além disso, embora não seja observável nos gráficos, nota-se um aumento
% da instabilidade para Kp's muito elevados (alguma oscilação em regime
% estacionário) pelo que é de evitar elevados valores. Tal deve-se à
% oscilação na leitura do sensor que pode atingir os 4º!
% Como conclusão pode-se referir a importancia deste parametro para o
% desaparecimento do overshoot (a partir de Kp=50).
%
% Variação de Kd:
% Variando este parametro apenas se observa uma suavização da resposta ao
% polinómio, mas nem o tempo de atraso nem o overshoot são resolvidos. A
% importancia desta componente apenas se prende com a capacidade de evitar
% pontos de inflexão durante o percurso da trajectória.

help testing_k.m

clear all
close all


%_____________________________________________________________
i0=load('Ki=0.05\traject=up_intMaxTorque#T=50#M=m4#ki005kp000kd000#t_pos_vel_curr_posTh_velTh.txt');
i1=load('Ki=0.01\traject=up_intMaxTorque#T=50#M=m4#ki001kp000kd000#t_pos_vel_curr_posTh_velTh.txt');
i2=load('Ki=0.10\traject=up_intMaxTorque#T=50#M=m4#ki010kp000kd000#t_pos_vel_curr_posTh_velTh.txt');
i3=load('Ki=0.20\traject=up_intMaxTorque#T=50#M=m4#ki020kp000kd000#t_pos_vel_curr_posTh_velTh.txt');

p1=load('Ki=0.05\Kp\traject=up_intMaxTorque#T=50#M=m4#ki005kp030kd000#t_pos_vel_curr_posTh_velTh.txt');
p2=load('Ki=0.05\Kp\traject=up_intMaxTorque#T=50#M=m4#ki005kp080kd000#t_pos_vel_curr_posTh_velTh.txt');
p3=load('Ki=0.05\Kp\traject=up_intMaxTorque#T=50#M=m4#ki005kp150kd000#t_pos_vel_curr_posTh_velTh.txt');

d1=load('Ki=0.05\Kd\traject=up_intMaxTorque#T=50#M=m4#ki005kp000kd020#t_pos_vel_curr_posTh_velTh.txt');
d2=load('Ki=0.05\Kd\traject=up_intMaxTorque#T=50#M=m4#ki005kp000kd050#t_pos_vel_curr_posTh_velTh#osc.txt');
d3=load('Ki=0.05\Kd\traject=up_intMaxTorque#T=50#M=m4#ki005kp000kd100#t_pos_vel_curr_posTh_velTh#osc.txt');

%Tempo
timei0=i0(:,1);
timei1=i1(:,1);
timei2=i2(:,1);
timei3=i3(:,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);
predictedi1=i1(:,5);
predictedi2=i2(:,5);
predictedi3=i3(:,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);
reali1=i1(:,2);
reali2=i2(:,2);
reali3=i3(:,2);

realp1=p1(:,2);             reald1=d1(:,2);
realp2=p2(:,2);             reald2=d2(:,2);
realp3=p3(:,2);             reald3=d3(:,2);

% Erro
errori0=predictedi0-reali0;
errori1=predictedi1-reali1;
errori2=predictedi2-reali2;
errori3=predictedi3-reali3;

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

figure;
axes('fontsize',14)
plot(timei0,predictedi0,'k',timei0,reali0,timep1,realp1,timep2,realp2,timep3,realp3,'linewidth',2);
xlabel('time (s)');
ylabel('position (degrees)');
title('Ki=0.05');
legend('Expected','Kp=0.00','Kp=0.30','Kp=0.80','Kp=1.50','location','southeast');
grid on

figure;
axes('fontsize',14)
plot(timei0,predictedi0,'k',timei0,reali0,timep1,realp1,timep2,realp2,timep3,realp3,'linewidth',2);
xlabel('time (s)');
ylabel('position (degrees)');
title('Ki=0.05');
legend('Expected','Kp=0.00','Kp=0.30','Kp=0.80','Kp=1.50','location','southeast');
grid on

figure('name','(Massa=675g) Variando Kp e Kd com Ki=0.05');
subplot(2,2,1);
plot(timei0,predictedi0,'k',timei0,reali0,timep1,realp1,timep2,realp2,timep3,realp3);
xlabel('time');
ylabel('position');
title('Variação de Kp (Kd=0)');
legend('Expected','Kp=0.00','Kp=0.30','Kp=0.80','Kp=1.50','location','southeast');

subplot(2,2,2);
plot(timei0,predictedi0,'k',timei0,reali0,timed1,reald1,timed2,reald2,timed3,reald3);
xlabel('time');
ylabel('position');
title('Variação de Kd (Kp=0)');
legend('Expected','Kd=0.00','Kd=0.20','Kd=0.50','Kd=1.00','location','southeast');

subplot(2,2,3);
plot(timei0,errori0,timep1,errorp1,timep2,errorp2,timep3,errorp3);
grid on
xlabel('time');
ylabel('error');

subplot(2,2,4);
plot(timei0,errori0,timed1,errord1,timed2,errord2,timed3,errord3);
grid on
xlabel('time');
ylabel('error');

%_____________________________________________________________
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 e Kd com Ki=0.10');
subplot(2,2,1);
plot(timei0,predictedi0,'k',timei0,reali0,timep1,realp1,timep2,realp2,timep3,realp3);
xlabel('time');
ylabel('position');
title('Variação de Kp (Kd=0)');
legend('Expected','Kp=0.00','Kp=0.25','Kp=0.50','Kp=1.10','location','southeast');

subplot(2,2,2);
plot(timei0,predictedi0,'k',timei0,reali0,timed1,reald1,timed2,reald2,timed3,reald3);
xlabel('time');
ylabel('position');
title('Variação de Kd (Kp=0)');
legend('Expected','Kd=0.00','Kd=0.30','Kd=0.80','Kd=1.50','location','southeast');

subplot(2,2,3);
plot(timei0,errori0,timep1,errorp1,timep2,errorp2,timep3,errorp3);
grid on
xlabel('time');
ylabel('error');

subplot(2,2,4);
plot(timei0,errori0,timed1,errord1,timed2,errord2,timed3,errord3);
grid on
xlabel('time');
ylabel('error');


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

p1=load('Ki=0.20\Kp\traject=up_intMaxTorque#T=50#M=m4#ki020kp070kd000#t_pos_vel_curr_posTh_velTh#THIS.txt');
p2=load('Ki=0.20\Kp\traject=up_intMaxTorque#T=50#M=m4#ki020kp150kd000#t_pos_vel_curr_posTh_velTh#osc.txt');

%Tempo
timei0=i0(:,1);
timep1=p1(:,1);
timep2=p2(:,1);

% Trajectória prevista
predictedi0=i0(:,5);
predictedp1=p1(:,5);
predictedp2=p2(:,5);

% Trajectória real
reali0=i0(:,2);
realp1=p1(:,2);
realp2=p2(:,2);

% Erro
errori0=predictedi0-reali0;
errorp1=predictedp1-realp1;
errorp2=predictedp2-realp2;

figure('name','(Massa=675g) Variando Kp com Ki=0.20');
subplot(2,1,1);
plot(timei0,predictedi0,'k',timei0,reali0,timep1,realp1,timep2,realp2);
xlabel('time');
ylabel('position');
title('Variação de Kp (Kd=0)');
legend('Expected','Kp=0.00','Kp=0.70','Kp=1.50','location','southeast');

subplot(2,1,2);
plot(timei0,errori0,timep1,errorp1,timep2,errorp2);
grid on
xlabel('time');
ylabel('error');
