% Controlador usado:
% posição(n)=posição(n-1+Kp*e(n),   e(n)=posição final-posição(n)
%
% Conclusões:
% Todas as medições foram feitas pela aplicação de uma rampa com uma carga
% de 933g (M1+M4).
%
% Figura1: Percurso:-90º=>0º, timeStep=500ms, positionStep=5º
% Observando a primeira figura (sem controlo) pode-se observar o crescente
% atraso que a posição do servo sofre ao longo do tempo a medida que
% aumentamos a posição solicitada. Introduzindo o controlo o atraso deixa
% de ser crescente e passa a ser fixo sendo tanto menor quanto maior for a
% constante Kp. Tal é explicado pelo facto de o erro (posição final-posição
% no instante) possibilitar uma compensação dinamica, obrigando a manter
% um atraso constante. No entanto deve-se notar que o valor de Kp deve ser
% limitado por forma a evitar overshoot e um comportamento em degrau.
% Por análise dos gráficos o melhor valor corresponde a Kp=0.10.
%
% Figura2: Percurso: -90º=>0º, timeStep=200ms, positionStep=5º
% Relativamente à figura anterior, apenas diminuimos o passo temporal de
% 500 para 200ms. Nesta figura podemos apreender o inconveniente de aplicar
% um Kp muito baixo (0.02) verificando-se uma resposta do tipo quadrática
% sem qualquer garantia de atraso constante. Também já se pode observar a
% ocorrência de overshoot para Kp's muito elevados.
%
% Figura3: Percurso: -90º=>0º, timeStep=100ms, positionStep=5º
% Reduzindo ainda mais o passo temporal (para 100ms) a oscilação começa a
% verificar-se mais cedo que nos casos anteriores o que nos leva a concluir
% que o valor de Kp é limitado pela máxima taxa de actualização das
% trajectórias. Para este caso Kp=0.01 já não é aceitável, apenas
% tolerando-se o valor de 0.07.
%
% Figura4: Percurso: -45º=>+45º, timeStep=100ms, positionStep=5º
% Nesta figura apenas modificámos a trajectória para o pecurso de -45º a
% +45º não se observando alterações significativas relativamente ao caso
% anterior.
%
% Conclusões finais:
% Podemos pois afirmar que na execução de rampas em trajectórias entre -90
% a +90º, um Kp=0.07 apresenta uma boa solução de compromisso sem provocar
% qualquer oscilação nem atraso significativo. De notar que este valor
% aplica-se para a carga de 933g sendo perfeitamente seguro usar este valor
% para cargas inferiores (nestes casos é possível usar maiores valores de
% Kp sem comprometer a estabilidade do sistema. Para cargas mais elevadas
% este valor tem de ser renegociado.

close all
clear all

help response_ramp

title1= 'Resposta à Rampa: -90º=>0º, timeStep=500ms, positionStep=5º, Carga=M1+M4=933g';
files1=['rampaM14nocontrol_posReq_posEff_kt0#-90;0_dt500.txt    ',
        'rampaM14kp=007_posReq_posEff_controlReq#-90;0_dt500.txt',
        'rampaM14kp=002_posReq_posEff_controlReq#-90;0_dt500.txt',
        'rampaM14kp=010_posReq_posEff_controlReq#-90;0_dt500.txt',
        'rampaM14kp=004_posReq_posEff_controlReq#-90;0_dt500.txt',
        'rampaM14kp=015_posReq_posEff_controlReq#-90;0_dt500.txt'];        
label1=['Sem Controlo',
        'Kp=0.07     ',
        'Kp=0.02     ',
        'Kp=0.10     ',
        'Kp=0.04     ',
        'Kp=0.15     '];

title2= 'Resposta à Rampa: -90º=>0º, timeStep=200ms, positionStep=5º, Carga=M1+M4=933g';
files2=['rampaM14nocontrol_posReq_posEff_kt0#-90;0_dt200.txt    ',
        'rampaM14kp=010_posReq_posEff_controlReq#-90;0_dt200.txt',
        'rampaM14kp=002_posReq_posEff_controlReq#-90;0_dt200.txt',
        'rampaM14kp=015_posReq_posEff_controlReq#-90;0_dt200.txt',
        'rampaM14kp=004_posReq_posEff_controlReq#-90;0_dt200.txt',
        'rampaM14kp=020_posReq_posEff_controlReq#-90;0_dt200.txt',
        'rampaM14kp=007_posReq_posEff_controlReq#-90;0_dt200.txt',
        'rampaM14kp=030_posReq_posEff_controlReq#-90;0_dt200.txt'];
label2=['Sem Controlo',
        'Kp=0.10     ',
        'Kp=0.02     ',
        'Kp=0.15     ',
        'Kp=0.04     ',        
        'Kp=0.20     ',
        'Kp=0.07     ',
        'Kp=0.30     '];

title3= 'Resposta à Rampa: -90º=>0º, timeStep=100ms, positionStep=5º, Carga=M1+M4=933g';
files3=['rampaM14nocontrol_posReq_posEff_kt0#-90;0_dt100.txt    ',
        'rampaM14kp=010_posReq_posEff_controlReq#-90;0_dt100.txt',
        'rampaM14kp=002_posReq_posEff_controlReq#-90;0_dt100.txt',
        'rampaM14kp=015_posReq_posEff_controlReq#-90;0_dt100.txt',
        'rampaM14kp=004_posReq_posEff_controlReq#-90;0_dt100.txt',
        'rampaM14kp=020_posReq_posEff_controlReq#-90;0_dt100.txt',
        'rampaM14kp=007_posReq_posEff_controlReq#-90;0_dt100.txt',
        'rampaM14kp=030_posReq_posEff_controlReq#-90;0_dt100.txt'];
label3=['Sem Controlo',
        'Kp=0.10     ',
        'Kp=0.02     ',
        'Kp=0.15     ',
        'Kp=0.04     ',
        'Kp=0.20     ',
        'Kp=0.07     ',
        'Kp=0.30     '];

title4= 'Resposta à Rampa: -45º=>+45º, timeStep=100ms, positionStep=5º, Carga=M1+M4=933g';
files4=['rampaM14nocontrol_posReq_posEff_kt0#-45;+45_dt100.txt    ',
        'rampaM14kp=010_posReq_posEff_controlReq#-45;+45_dt100.txt',
        'rampaM14kp=002_posReq_posEff_controlReq#-45;+45_dt100.txt',
        'rampaM14kp=015_posReq_posEff_controlReq#-45;+45_dt100.txt',
        'rampaM14kp=004_posReq_posEff_controlReq#-45;+45_dt100.txt',
        'rampaM14kp=020_posReq_posEff_controlReq#-45;+45_dt100.txt',
        'rampaM14kp=007_posReq_posEff_controlReq#-45;+45_dt100.txt',
        'rampaM14kp=030_posReq_posEff_controlReq#-45;+45_dt100.txt'];
label4=['Sem controlo',
        'Kp=0.10     ',
        'Kp=0.02     ',
        'Kp=0.15     ',
        'Kp=0.04     ',
        'Kp=0.20     ',
        'Kp=0.07     ',
        'Kp=0.30     '];

legenda=['Posição solicitada   ',
         'Percurso sem controlo',
         'Percurso COM controlo'];
%         'Pedido do controlador'];

f4=figure(4);
set(f4,'name',title4);

for i=1:size(files4,1)
    clear array;
    array=load(files4(i,:));
    
    subplot(4,2,i);
    if (i==1)
        array0=array;
        plot(array0(:,1),array0(:,2),'g',array0(:,1),array0(:,3),'b');
    else
        plot(array(:,1),array(:,2),'g',array0(:,1),array0(:,3),'b',...
             array(:,1),array(:,3),'r');
%         plot(array(:,1),array(:,2),'g',array0(:,1),array0(:,3),'b',...
%              array(:,1),array(:,3),'r',array(:,1),array(:,4),'y');
    end
    
    axis([0,5,-50,50]);
    xlabel(label4(i,:));
    ylabel('Posição (graus)');
    
    if (i==size(files4,1))
        legend(legenda,'location','southeast');
    end
    grid on
end

f3=figure(3);
set(f3,'name',title3);

for i=1:size(files3,1)
    clear array;
    array=load(files3(i,:));
    
    subplot(4,2,i);
    if (i==1)
        array0=array;
        plot(array0(:,1),array0(:,2),'g',array0(:,1),array0(:,3),'b');
    else
        plot(array(:,1),array(:,2),'g',array0(:,1),array0(:,3),'b',...
             array(:,1),array(:,3),'r');
%         plot(array(:,1),array(:,2),'g',array0(:,1),array0(:,3),'b',...
%              array(:,1),array(:,3),'r',array(:,1),array(:,4),'y');
    end
    
    axis([0,5,-100,10]);
    xlabel(label3(i,:));
    ylabel('Posição (graus)');
    
    if (i==size(files3,1))
        legend(legenda,'location','southeast');
    end
    grid on
end

f2=figure(2);
set(f2,'name',title2);

for i=1:size(files2,1)
    clear array;
    array=load(files2(i,:));
    
    subplot(4,2,i);
    if (i==1)
        array0=array;
        plot(array0(:,1),array0(:,2),'g',array0(:,1),array0(:,3),'b');
    else
        plot(array(:,1),array(:,2),'g',array0(:,1),array0(:,3),'b',...
             array(:,1),array(:,3),'r');
%         plot(array(:,1),array(:,2),'g',array0(:,1),array0(:,3),'b',...
%              array(:,1),array(:,3),'r',array(:,1),array(:,4),'y');
    end
    
    axis([0,7,-100,10]);
    xlabel(label2(i,:));
    ylabel('Posição (graus)');
    
    if (i==size(files2,1))
        legend(legenda,'location','southeast');
    end
    grid on
end


f1=figure(1);
set(f1,'name',title1);

for i=1:size(files1,1)
    clear array;
    array=load(files1(i,:));
    
    subplot(3,2,i);
    if (i==1)
        array0=array;
        plot(array0(:,1),array0(:,2),'g',array0(:,1),array0(:,3),'b');
    else
        plot(array(:,1),array(:,2),'g',array0(:,1),array0(:,3),'b',...
             array(:,1),array(:,3),'r');
%         plot(array(:,1),array(:,2),'g',array0(:,1),array0(:,3),'b',...
%              array(:,1),array(:,3),'r',array(:,1),array(:,4),'y');
    end
    
    axis([0,15,-100,10]);
    xlabel(label1(i,:));
    ylabel('Posição (graus)');
    
    if (i==size(files1,1))
        legend(legenda,'location','southeast');
    end
    grid on
end
