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

% Planeamento de trajectorias ponto a ponto

close all
clear all

% Sampling Time
ts=1e-3;

% Propriedades do duplo pendulo
pendulo2_data;

% Angulos iniciais das juntas
theta1_0=0.4*pi;
theta2_0=0.2*pi;

% Leitura da trajectoria pelo utilizador
N=input('Numero de pontos de referencia da trajectoria: ');
[tref,x,y]=getPositions(N,theta1_0,theta2_0,L1,L2);

% Cinematica inversa dos pontos de referencia
[theta1,theta2]=cinematInv2(x,y,L1,L2);

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

    differ=theta2(i)-theta2(i-1);
    if (differ>pi)
        theta2(i:end)=theta2(i:end)-2*pi;
    elseif (differ<-pi)
        theta2(i:end)=theta2(i:end)+2*pi;
    end
end

% Tempo total de simulaçao
t0=tref(1);
t_sim=tref(end);
t=[t0:ts:t_sim];

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

% Calculo do array de posiçoes do pendulo para cada instante de tempo t
% (feito offline)
pos1=[];                            % Para o primeiro elo
pos2=[];                            % Para o segundo elo
for i=2:length(tref)
    t_int=[tref(i-1):ts:tref(i)-ts];
    p1=geradortraj(t_int,[theta1(i-1) theta1(i)],[0 0],[0 0]);
    p2=geradortraj(t_int,[theta2(i-1) theta2(i)],[0 0],[0 0]);
    
    pos1=[pos1 p1];
    pos2=[pos2 p2];
end
pos1=[pos1 pos1(end)];
pos2=[pos2 pos2(end)];


% Salvaguarda do comprimento dos elos (para posterior animaçao)
save pendulo_length L1 L2

% Chamada do modelo simulink
pendulo2_simulink;

input('Pressione ENTER para correr a animaçao...');

% Animaçao e analise de resultados
analyse_exe