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

close all
clear all

% Sampling Time
ts=1e-3;

% As propriedades do sistema sao definidas directamente no bloco do pendulo
% simples!

% 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.
theta_list=[-pi/2     0;
             pi       2];

% Angulo inicial do pendulo (a ser usado como condiçao inicial no sistema
% real).
theta0=theta_list(1,1);

% Tempo total de simulaçao
t0=theta_list(1,2);
t_sim=theta_list(end,2);
t=t0:ts:t_sim;

% Definiçao do array de posiçoes do pendulo como global
global pos;

% Calculo do array de posiçoes do pendulo para cada instante de tempo t
% (feito offline)
pos=[];
for i=2:size(theta_list,1)
    t_int=[theta_list(i-1,2):ts:theta_list(i,2)];
    p=geradortraj(t_int,[theta_list(i-1,1) theta_list(i,1)],[0 0],[0 0]);

    pos=[pos p];
end

% Chamada do modelo simulink
pendulum_simulink_digcontroller;

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,1,1);
        plot(t,pos,t,pos_real)
        grid on
        title('Predicted trajectory (blue)  VS  Real trajectory (green)');
        xlabel('t');
        ylabel('theta');

        error=pos-pos_real';
        subplot(2,1,2);
        plot(t,error,'r')
        grid on
        title('Trajectory error');
        xlabel('t');
        ylabel('error');
        
        fprintf('Maximum Error= %g\n',max(abs(error)));
    end
end