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

% Esta versao resolve varios bugs que as versoes anteriores possuiam, e
% acrescenta algumas modificaçoes. Esta versao pretende-se servir de base a
% todas as posteriores!
% Bugs resolvidos:
% - Para listas de trajectorias com mais de dois pontos, e garantido a
%   mesma dimensao dos arrays de posiçao que para os obtidos na simulaçao.
% - No modelo simulink, a componente força externa ja possui o pontos de
%   aplicaçao variaveis (definidos offline) - rfx em vez de rx.
% Modificaçoes:
% - A optimizaçao de trajectoria foi removida por nao se apresentar
%   necessaria.
% - A analise do erro ja nao e feita no ficheiro de inicializaçao, mas num
%   aparte.
% - O modelo simulink ja nao efectua animaçao. Apenas armazena os
%   resultados num ficheiro mat.
% - A animaçao e realizada por execuçao de uma funçao posteriormente a
%   realizaçao da simulaçao.

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=[-pi/2     0;
              pi/2     1;
              pi       2;
              0        3];

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

% 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)];

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

% Salvaguarda da trajectoria prevista
trajectory_expected=[t; pos1; pos2];
save trajectory_expected.mat trajectory_expected;

% Chamada do modelo simulink
pendulo2_simulink;

fprintf('Para animar o movimento do pendulo e fazer a sua analise, execute a funçao ''analyse_exe''.\n');