% Script MatLab para demonstraçao da execuçao de trajectorias no pendulo
% controlado por PID

% Esta versao optimiza a execuçao da trajectoria fazendo-o percorrer a
% trajectoria mais curta.

close all
clear all

% Sampling Time
ts=1e-3;

% Propriedades do duplo pendulo
pendulo2_data;

% Definiçao do array de posiçoes do pendulo como global
global pos1 pos2

% Lista de posiçoes que o pendulo devera assumir ao longo do tempo.
% Coluna 1: Lista de angulos
% Coluna 2: Tempo no qual o angulo devera ser atingido.
% ATENÇAO: E importante que estes valores estejam na gama ]-pi,pi].
theta1_list=[ 0        0;
              pi       1];

theta2_list=[-0.1*pi   0;
             -pi/2     1];

% Optimizaçao da trajectoria (para execuçao do menor percurso)
% (Bloco de codigo a verificar)
for i=2:size(theta1_list,1)
    differ=theta1_list(i,1)-theta1_list(i-1,1);
    if (differ>pi)
        theta1_list(i:end,1)=theta1_list(i:end,1)-2*pi;
    elseif (differ<-pi)
        theta1_list(i:end,1)=theta1_list(i:end,1)+2*pi;
    end
end
for i=2:size(theta2_list,1)
    differ=theta2_list(i,1)-theta2_list(i-1,1);
    if (differ>pi)
        theta_list2(i:end,1)=theta2_list(i:end,1)-2*pi;
    elseif (differ<-pi)
        theta_list2(i:end,1)=theta2_list(i:end,1)+2*pi;
    end
end

% Angulo inicial do pendulo (a ser usado como condiçao inicial no sistema
% real).
theta1_0=theta1_list(1,1);
theta2_0=theta2_list(1,1);

% Tempo total de simulaçao
t0=max([theta1_list(1,2),theta2_list(1,2)]);
t_sim=min(theta1_list(end,2), theta2_list(end,2));

% Periodo de tempo
t=[t0:ts:t_sim];

% Calculo do array de posiçoes do pendulo para cada instante de tempo t
% (feito offline)
pos1=[];                            % Para o primeiro elo
for i=2:size(theta1_list,1)
    t_int=[theta1_list(i-1,2):ts:theta1_list(i,2)-ts];
    p=geradortraj(t_int,[theta1_list(i-1,1) theta1_list(i,1)],[0 0],[0 0]);
    
    pos1=[pos1 p];
end
pos1=[pos1 pos1(end)];

pos2=[];                            % Para o segundo elo
for i=2:size(theta2_list,1)
    t_int=[theta2_list(i-1,2):ts:theta2_list(i,2)-ts];
    p=geradortraj(t_int,[theta2_list(i-1,1) theta2_list(i,1)],[0 0],[0 0]);
    
    pos2=[pos2 p];
end
pos2=[pos2 pos2(end)];

% Chamada do modelo simulink
pendulo2_simulink;

answer='';
while (length(answer)~=1 || (answer~='q' && answer~='Q'))
    % Esperando que o utilizador pressione ENTER
    answer=input('Introduza ENTER para analisar resultados/''q'' para sair...','s');

    if (length(answer)~=1 || (answer~='q' && answer~='Q'))
        % Visualizaçao do erro entre a trajectoria esperada e a real
        close all
        subplot(2,2,1);
        plot(t,pos1,t,pos1_real)
        grid on
        title('Predicted (blue) VS Real trajectory (green)');
        xlabel('t');
        ylabel('theta1');

        subplot(2,2,2);
        plot(t,pos2,t,pos2_real)
        grid on
        title('Predicted (blue) VS Real trajectory (green)');
        xlabel('t');
        ylabel('theta2');

        error1=pos1-pos1_real';
        subplot(2,2,3);
        plot(t,error1,'r')
        grid on
        title('Trajectory error');
        xlabel('t');
        ylabel('error1');
        fprintf('Maximum Error (theta1)= %g\n',max(abs(error1)));
        
        error2=pos2-pos2_real';
        subplot(2,2,4);
        plot(t,error2,'r')
        grid on
        title('Trajectory error');
        xlabel('t');
        ylabel('error2');
        fprintf('Maximum Error (theta2)= %g\n',max(abs(error2)));
    end
end