% Simulação do algoritmo de geração de trajectórias nas unidades Slave
%
% [thetan,omegan,gaman,a]=gerar_0(theta0,thetaF,T,w0)
%
% theta0 -> Posição angular inicial (graus)
% thetaF -> Posição angular final (graus)
% T      -> Duração do movimento em ticks (ciclos de 20ms) (*2 ticks)
% w0     -> Velocidade angular inicial (apenas para T=0) (*0.01 graus/tick)
%
% Caso T==0, uma trajectória de paragem é gerada a partir da velocidade
% inicial w0.
%
% thetan <- Trajectória de posição
% omegan <- Trajectória de velocidade
% gaman  <- Trajectória de aceleração
% a      <- Coeficientes do polinómio da posição angular em função de t.

function [thetan,omegan,gaman,a]=gerar_0(theta0,thetaF,T,w0)

% Constantes
WMAX=360;                   % Velocidade máxima do servo (*0.01 graus/tick)
GSTOP=5;                    % Desaceleração de paragem (*0.01 graus/tick^2)
PER_FACTOR=2;               % Factor de ajuste do período do movimento

% Variação angular de deslocamento
deltaTheta=thetaF-theta0;

% Se o período for nulo, passar a fase de paragem
if (T==0)
    period=abs(w0)/GSTOP;           % Cálculo da duração da paragem
    
    % Coeficientes do polinómio
    a0=theta0;                      % Posição inicial
    a1=w0/100;                      % Velocidade inicial normalizada
    a2=-((w0>=0)*2-1)*GSTOP/200;    % Parametro proporcional à aceleração
    a3=0;                           % Parametro proporcional à derivada da aceleração
    
% Trajectória normal
else
    period=T*PER_FACTOR;                        % Ajuste do período do movimento
    
    wmax=3*abs(deltaTheta)*100/(2*period);      % Cálculo da velocidade máxima
    if (wmax>WMAX)                              % Se a velocidade máxima ultrapassa
        period=3*abs(deltaTheta)*100/(2*WMAX);  % ... a máxima estipulada, limitá-la!
    end
    
    % Coeficientes do polinómio
    a0=theta0;                                  % Posição inicial
    a1=0;                                       % Velocidade inicial normalizada
    a2=3*deltaTheta/period^2;                   % Parametro proporcional à aceleração
    a3=-2*deltaTheta/period^3;                  % Parametro proporcional à derivada da aceleração
end

% Coeficientes do polinómio
a=[a0 a1 a2 a3];

% Gama temporal
tn=0:1:period;

% Resultados simbólicos
syms t
theta=a0+a1*t+a2*t.^2+a3*t.^3;      % Posição
omega=diff(theta,t);                % Velocidade
gama =diff(omega,t);                % Aceleração

% Resultados numéricos
thetan=subs(theta,t,tn);
omegan=subs(omega,t,tn);
gaman =subs(gama ,t,tn);

% Figuras
figure(3);
plot(tn,gaman);                     % Aceleração angular
figure(2);
plot(tn,omegan);                    % Velocidade angular
figure(1);
plot(tn,thetan);                    % Posição
