// ~~~~ ANÁLISE DE RESPOSTA AO DEGRAU (ÚNICO E MULTIPLO)~~~~ 
clear

// PARÂMETROS
g = 9.81; // [m/s2]
Mt = 0.82; // [kg]
Iy = 8.1*(10^-3); // [kgm2]
Ce = 0.1154;
l = 0.29; // [m]

// VARIÁVEIS
w1 = 1; // velocidade angular do rotor 1 [rad/s]
w3 = 1; // velocidade angular do rotor 3 [rad/s]

// TEMPO
t0 = 0; //tempo inicial [s]
dt = 0.01; //passo de integração [s]
tspan = 10; //intervalo de integração [s]
t = t0:dt:tspan; //vetor tempo [s]

// CONDIÇÕES INICIAIS PARA O REFERENCIAL MÓVEL
x0 = 0; // posição x [m]
xp0 = 0; // velocidade em x [m/s]
z0 = 0; // posição em z [m]
zp0 = 0; // velocidade em z [m/s]
theta0 = 0; // posição em torno de y [rad]
thetap0 = 0; // velocidade em torno de y [rad/s]

X0 = [x0; xp0; z0; zp0; theta0; thetap0]; // vetor das condições iniciais

// ENTRADAS
tipo = 'degrau unico' // 'degrau unico' ou 'degrau multiplo'

t0_U = 2; // tempo inicial de entrada para u1a
tfa_U = 4; // tempo final de entrada para u1a - início de u1b
tfb_U = 6; // tempo final de entrada para u1b - início de u1c
tfc_U = 8; // tempo final de entrada para u1c - início de u1d
tfd_U = 10; // tempo final de entrada para u1d

// u1 -> empuxo; u3 --> momento de arfagem
u1a = 1.1*Mt*g; // 1.1Mtg para entrada unica e 2Mtg para entradas múltiplas - u1a
u1b = -2*Mt*g; // para entradas múltiplas - u1b
u1c = 4*Mt*g; // para entradas múltiplas - u1c
u1d = 0; // para entradas múltiplas - u1d
u1_nominal = Mt*g;
u3 = Ce*l*(w3^2 - w1^2); //
u3_nominal = 0;

//definição do vetor das entradas U = [u1;u3] em função do tempo

if tipo == 'degrau unico' then
function entradas = U(t) 
        if t >= t0_U 
            entradas = [u1a;u3]
        else 
            entradas = [u1_nominal;u3_nominal]
        end
endfunction
end

if tipo == 'degrau multiplo' then
function entradas = U(t) // para mais de um degrau ao longo do tempo
    //verifica o tipo de entrada desejado
        if t >= t0_U then 
            if t < tfa_U then
                entradas = [u1a; u3];
            elseif t < tfb_U then
                entradas = [u1b; u3];
            elseif t < tfc_U then
                entradas = [u1c; u3];
            elseif t < tfd_U then
                entradas = [u1d; u3];
            else
                entradas = [u1_nominal;u3_nominal];
            end
        else 
            entradas = [u1_nominal;u3_nominal];
        end
endfunction
end

// ESPAÇO DE ESTADOS

funcprot(0);
    function dy = VANT(t,y,U)
        dy(1) = y(2)
        dy(2) = (1/Mt)*sin(y(5))*U(t)(1)
        dy(3) = y(4)
        dy(4) = - g +((1/Mt)*cos(y(5))*U(t)(1))
        dy(5) = y(6)
        dy(6) = (1/Iy)*U(t)(2)
    endfunction;

// RESULTADOS NO REFERENCIAL MÓVEL
X = ode(X0,t0,t,VANT);
xp = X(2,:) // velocidade em x [m/s]
zp = X(4,:) // velocidade em z [m/s]
theta = X(5,:) // arfagem (em torno de y) [rad]
thetap = X(6,:) // velocidade de arfagem (em torno de y) [rad/s]

// MUDANÇA PARA O REFERENCIAL FIXO
xpInercial = cos(theta).*xp + sin(theta).*zp // velocidade de avanço no SC inercial [m/s]
zpInercial = - sin(theta).*xp + cos(theta).*zp // velocidade de guinada no SC inercial [m/s]

// INTEGRAÇÃO (velocidades -> posições)
function F = integra(f, t, F0) 
    F = zeros(f) 
    F(1) = F0 // definição da condição inicial
    for i = 2:1:length(f)
        F(i) = inttrap(t(1:i),f(1:i)) // integração pelo método dos trapézios
    end
endfunction

xInercial = integra(xpInercial,t,0) // posição x no referencial fixo [m]
zInercial = integra(zpInercial,t,0) + 0.3 // posição z no referencial fixo [m]
// 0.3 é adicionado em zInercial devido à condição de equilíbrio adotada
thetaInercial = theta // posição angular em torno de y (arfagem)[rad]

//~~ PLOTAGENS ~~

xset('window',1)
plot2d(t,xp)
e=gce()
e.children.thickness=2
e.children.foreground=2
xtitle('$\LARGE Velocidade\: em\: X$','$\Large t\: [s]$','$\Large \dot X_{E}\: [m/s]$');
xgrid(1)

xset('window',2)
plot2d(t,xInercial)
e=gce()
e.children.thickness=2
e.children.foreground=2
xtitle('$\LARGE Deslocamento\: em\: X\:$','$\Large t\: [s]$','$\Large X_{E}\: [m]$')
xgrid(1)

xset('window',3)
plot2d(t,zp)
e=gce()
e.children.thickness=2
e.children.foreground=2
xtitle('$\LARGE Velocidade\: em\: Z$','$\Large t\: [s]$','$\Large \dot Z_{E}\: [m/s]$')
xgrid(1)

xset('window',4)
plot2d(t,zInercial)
e=gce()
e.children.thickness=2
e.children.foreground=2
xtitle('$\LARGE Deslocamento\: em\: Z $','$\Large t\: [s]$','$\Large Z_{E}\: [m]$')
xgrid(1)

xset('window',5)
plot2d(t,thetap)
e=gce()
e.children.thickness=2
e.children.foreground=2
xtitle('$\LARGE Velocidade\: de\: Arfagem$','$\Large t\: [s]$','$\Large \dot \theta_{E}\: [rad/s]$')
xgrid(1)

xset('window',6)
plot2d(t,thetaInercial)
e=gce()
e.children.thickness=2
e.children.foreground=2
xtitle('$\LARGE Arfagem$','$\Large t\: [s]$','$\Large \theta_{E}\: [rad]$')
xgrid(1)

xset('window',7)
plot2d(xInercial,zInercial)
e=gce()
e.children.thickness=2
e.children.foreground=2
xtitle('$\LARGE Trajetória$','$\Large X_{E}\: [m]$','$\Large Z_{E}\: [m]$')
xgrid(1)