function [QQ QQV QQA tt]=PolyTraj5taOrd(Q0,Qf,N,t0,tf)
%% funcao que retorna uma trajetoria polinomial de quinta ordem entre duas posturas com velocidades e aceleracoes iniciais e finais distintas
% -> INPUT
% Q0: vector da posicao inicial das juntas
% Qf: vector da posicao final das juntas
% t0: instante inicial da trajectoria (segundos)
% tf: instante final da trajectoria (segundos)
% N: numero de pontos a usar na trajectoria
% -> OUTPUT
% QQ: matriz com N colunas onde em cada coluna contem os valores das juntas
%     nos instantes correspondentes.
% t: um vector com os instantes de tempo correspondentes aos N instantes
%    de amostragem da trajectoria

tt=linspace(t0,tf,N);
a3=-(10*(Q0 - Qf))/(tf-t0)^3;
a4=(15*(Q0 - Qf))/(tf-t0)^4;
a5=-(6*(Q0 - Qf))/(tf-t0)^5;
for i=1:N
    t=tt(i)-t0;
    QQ(:,i) = Q0 + a3*t^3 + a4*t^4 + a5*t^5;
    QQV(:,i) = 3*a3*t^2 + 4*a4*t^3 + 5*a5*t^4;
    QQA(:,i) = 6*a3*t + 12*a4*t^2 + 20*a5*t^3;
end