//Projeto final - Modelagem de Sistemas Dinâmicos

//Integrantes:
//Cássio Murakami;
//Gabriel Barbosa Paganini;
//Henrique Kuhlmann;
//João Otávio Tanaka de Oliveira;

clear();
xdel(winsid());

//Parâmetros de simulação:
t_inicial = 0;       //Instante inicial [s]
t_final = 5;        //Instante final [s]
iteration = 10000;    //Número de Discretizações
t = linspace(t_inicial,t_final,iteration);

t_desl = t_final;      //Instante que os dois motores desligam simultaneamente
t_desl_x = t_desl;     //Instante de desligamento do motor X
t_desl_y = t_desl;     //Instante de desligamento do motor Y

analise = 1; // "1" Para a comparação da solução não linear com linear | "2" Para a comparação da solução com motor elétrico e sem motor elétrico

//Parâmetros do projeto do satélite:
Ix = 9840;     //Momento de inércia em relação ao eixo x [kg m^2]
Iy = 9558;     //Momento de inércia em relação ao eixo y [kg m^2]
Iz = 2520;     //Momento de inércia em relação ao eixo z [kg m^2]
//Observação: (Ix >= Iy >= Iz)

//Parâmetros do projeto dos motores:
K  = 10;       //Constante do motor K
Kb = 0.001;        //Constante do motor Kb
R = 4;         //Resistência elétrica do motor [Ohm]
L = 0.001;          //Indutância do motor [H]
J = 0.7;          //Momento de inércia do rotor [kg m^2]

//Definição da evolução temporal das fontes de tensão:
function fun = Vx(t)
    if t < t_desl_x then
        fun = 48   //Definição da fonte de tensão do motor X
    else 
        fun = 0
    end
endfunction

function fun = Vy(t)
    if t < t_desl_y then
        fun = 48   //Definição da fonte de tensão do motor Y
    else
        fun = 0
    end
endfunction 

//Condições iniciais do sistema:
wx0 = 0.1;        //Componente do eixo X do vetor rotação instantânea [rad/s]
wy0 = 0.1;        //Componente do eixo Y do vetor rotação instantânea [rad/s]
wz0 = 1;        //Componente do eixo Z do vetor rotação instantânea [rad/s]

ix0 = 0;        // Corrente inicial no motor X [A]
iy0 = 0;        // Corrente inicial no motor Y [A]
iz0 = 0;        // Corrente inicial no motor Z [A]

hx0 = Vx(0)*J/Kb;        // Momento da quantidade de movimento inicial de X [kg m^2/s]
hy0 = Vy(0)*J/Kb;        // Momento da quantidade de movimento inicial de Y [kg m^2/s]

//Condições de operação:
wz_op = 0;
hx_op = Vx(0)*J/Kb;
hy_op = Vy(0)*J/Kb;

//Definição dos vetores de solução:
wx = zeros(length(t)); 
wy = zeros(length(t));
wz = zeros(length(t));

ix = zeros(length(t));
iy = zeros(length(t));

hx = zeros(length(t));
hy = zeros(length(t));

//Iniciação dos valores de desvio padrão:
des_wx = 0;
des_wy = 0;
des_wz = 0;
des_ix = 0;
des_iy = 0;
des_hx = 0;
des_hy = 0;
// ----------------- Solução do sistema não linear ----------------- //
    
//Definição do vetor de estados:
funcprot(0);
    function dy = solution_nl(t,y)
        wx = y(1);
        wy = y(2);
        wz = y(3);
        ix = y(4);
        iy = y(5);
        hx = y(6);
        hy = y(7);
        
        dy(1) = ((Iy + J - Iz)/Ix)*wy*wz + (1/Ix)*hy*wz - (K/Ix)*ix;
        dy(2) = ((Iz - Ix - J)/Iy)*wx*wz - (1/Iy)*hx*wz - (J/Iy)*iy;
        dy(3) = ((Ix - Iy)/Iz)*wx*wy + (1/Iz)*hx*wy - (1/Iz)*hy*wx;
        
        dy(4) =  -(R/L)*ix - (Kb/(J*L))*hx + (1/L)*Vx(t);
        dy(5) =  -(R/L)*iy - (Kb/(J*L))*hy + (1/L)*Vy(t);

        dy(6) =  -(J*(Ix + J - Iz)/Ix)*wy*wz - (J/Ix)*hy*wz + (K*(Ix+J)/Ix)*ix;
        dy(7) =  -(J*(Iz - Ix - J)/Iy)*wx*wz + (J/Iy)*hx*wz + (K*(Iy+J)/Iy)*iy;
    endfunction

result_nl = ode([wx0;wy0;wz0;ix0;iy0;hx0;hy0],0,t,solution_nl);

wx = result_nl(1,:);
wy = result_nl(2,:);
wz = result_nl(3,:);
ix = result_nl(4,:);
iy = result_nl(5,:);
hx = result_nl(6,:);
hy = result_nl(7,:);

// ------------------- Solução do sistema linear ------------------- //

funcprot(0)
    function dy = solution_l(t,y)
        x1 = y(1);
        x2 = y(2);
        x3 = y(3);
        x4 = y(4);
        x5 = y(5);
        x6 = y(6);
        x7 = y(7);
        
        dy(1) = ((Iy + J - Iz)*wz_op/Ix)*x2 + (hy_op/Ix)*x3 - (K/Ix)*x4 + (wz_op/Ix)*x7;
        dy(2) = ((Iz - Ix - J)*wz_op/Iy)*x1 - (hx_op/Iy)*x3 - (K/Iy)*x5 - (wz_op/Iy)*x6;
        dy(3) =  -(hy_op/Iz)*x1 + (hx_op/Iz)*x2;
        dy(4) = -(R/L)*x4 - (Kb/(J*L))*x6 + (1/L)*Vx(t);
        dy(5) = -(R/L)*x5 - (Kb/(J*L))*x7 + (1/L)*Vy(t);
        dy(6) = ((Iz - Ix - J)*J*wz_op/Ix)*x2 - (J*hy_op/Ix)*x3 + ((K*Ix + J*K)/Ix)*x4 - (J*wz_op/Ix)*x7;
        dy(7) = ((Ix + J - Iz)*J*wz_op/Iy)*x1 + (J*hx_op/Iy)*x3 + ((K*Iy + J*K)/Iy)*x5 + (J*wz_op/Iy)*x6;
    endfunction
    
result_l = ode([wx0;wy0;wz0;ix0;iy0;hx0;hy0],0,t,solution_l);

wx_l = result_l(1,:);
wy_l = result_l(2,:);
wz_l = result_l(3,:);
ix_l = result_l(4,:);
iy_l = result_l(5,:);
hx_l = result_l(6,:);
hy_l = result_l(7,:);

//Cálculo do desvio padrão entre a solução não linear e linear:

for i = 1:length(t)
    des_wx = des_wx + (wx(i) - wx_l(i))^2;
    des_wy = des_wy + (wy(i) - wy_l(i))^2;
    des_wz = des_wz + (wz(i) - wz_l(i))^2;
    des_ix = des_ix + (ix(i) - ix_l(i))^2;
    des_iy = des_iy + (iy(i) - iy_l(i))^2;
    des_hx = des_hx + (hx(i) - hx_l(i))^2;
    des_hy = des_hy + (hy(i) - hy_l(i))^2;
end

des_wx = sqrt(des_wx);
des_wy = sqrt(des_wy);
des_wz = sqrt(des_wz);
des_ix = sqrt(des_ix);
des_iy = sqrt(des_iy);
des_hx = sqrt(des_hx);
des_hy = sqrt(des_hy);

// ----------- Solução do sistema sem o sistema elétrico e rodas de reação ----------- //

//Definição do vetor de estados:
funcprot(0);
function dy = sat_nat(t,y)
    wx = y(1);
    wy = y(2);
    wz = y(3);
    
    dy(1) = ((Iy - Iz)/Ix)*wy*wz;
    dy(2) = ((Iz - Ix)/Iy)*wx*wz;
    dy(3) = ((Ix - Iy)/Iz)*wx*wy;
endfunction

result_nat = ode([wx0;wy0;wz0],0,t,sat_nat);

wx_nat = result_nat(1,:);
wy_nat = result_nat(2,:);
wz_nat = result_nat(3,:);

// -------------------- Exibição dos resultados -------------------- //

if analise == 1 then
    scf(1)
    subplot(2,1,1);
    xtitle("Rotação instantânea X do satélite");
    xlabel("Tempo [s]");
    ylabel("Rotação instantânea [rad/s] ");
    plot(t,wx,'b');
    plot_wx_l = plot(t,wx_l,'b');
    plot_wx_l.line_style = 5;
    plot_wx_l.foreground = 18;
    legend(['Rotação em X não linear';'Rotação em X linear']);
    
    subplot(2,1,2);
    xtitle("Rotação instantânea Y do satélite");
    xlabel("Tempo [s]");
    ylabel("Rotação instantânea [rad/s] ");
    plot(t,wy,'r');
    plot_wy_l = plot(t,wy_l,'r');
    plot_wy_l.line_style = 5;
    plot_wy_l.foreground = 6;
    legend(['Rotação em Y não linear';'Rotação em Y linear']);
    
    scf(2)
    xtitle("Rotação instantânea Z do satélite");
    xlabel("Tempo [s]");
    ylabel("Rotação instantânea [rad/s] ")
    plot_wz = plot(t,wz);
    plot_wz.foreground = 3;
    plot_wy_l = plot(t,wz_l);
    plot_wy_l.line_style = 5;
    plot_wy_l.foreground = 32;
    legend(['Rotação em Z não linear';'Rotação em Z linear']);
    
    scf(3)
    subplot(2,1,1);
    xtitle("Corrente do motor X ");
    xlabel("Tempo [s]");
    ylabel("Corrente elétrica [A] ");
    plot(t,ix,'b');
    plot_ix_l = plot(t,ix_l,'b');
    plot_ix_l.line_style = 5;
    plot_ix_l.foreground = 18;
    legend(['Corrente em X não linear';'Corrente em X linear']);
    
    subplot(2,1,2);
    xtitle("Corrente do motor Y");
    xlabel("Tempo [s]");
    ylabel("Corrente elétrica [A] ");
    plot(t,iy,'r');
    plot_iy_l = plot(t,iy_l,'r');
    plot_iy_l.line_style = 5;
    plot_iy_l.foreground = 6;
    legend(['Corrente em Y não linear';'Corrente em Y linear']);
    
    scf(4)
    subplot(2,1,1);
    xtitle("Momento da quantidade de movimento do rotor X");
    xlabel("Tempo [s]");
    ylabel("Momento da quantidade de movimento [kg m^2/s] ");
    plot(t,hx,'b');
    plot_hx_l = plot(t,hx_l,'b');
    plot_hx_l.line_style = 5;
    plot_hx_l.foreground = 18;
    legend(['Rotação do rotor X não linear';'Rotação do rotor X linear'],[4]);
    
    subplot(2,1,2);
    xtitle("Momento da quantidade de movimento do rotor Y");
    xlabel("Tempo [s]");
    ylabel("Momento da quantidade de movimento [kg m^2/s] ");
    plot(t,hy,'r');
    plot_hy_l = plot(t,hy_l,'r');
    plot_hy_l.line_style = 5;
    plot_hy_l.foreground = 6;
    legend(['Rotação do rotor Y não linear';'Rotação do rotor Y linear'],[4]);
    
    //Display dos desvios padrões das medidas:
    disp("Desvio padrão de wx : "+string(des_wx));
    disp("Desvio padrão de wy : "+string(des_wy));
    disp("Desvio padrão de wz : "+string(des_wz));
    disp("Desvio padrão de ix : "+string(des_ix));
    disp("Desvio padrão de iy : "+string(des_iy));
    disp("Desvio padrão de Omegax : "+string(des_hx));
    disp("Desvio padrão de Omegay : "+string(des_hy));
    
elseif analise == 2 then
    scf(1);
    xtitle("Comparação efeito do motor elétrico e rodas de reação em wx");
    xlabel("Tempo [s]");
    ylabel("Rotatão instantânea em x [rad/s] ");
    plot(t,wx,'r');
    plot(t,wx_nat,'black');
    legend(['Motores elétricos ligados';'Motores elétricos desligados']);
    
    scf(2);
    xtitle("Comparação efeito do motor elétrico e rodas de reação em wy");
    xlabel("Tempo [s]");
    ylabel("Rotatão instantânea em y [rad/s] ");
    plot(t,wy,'r');
    plot(t,wx_nat,'black');
    legend(['Motores elétricos ligados';'Motores elétricos desligados']);
    
    scf(3);
    xtitle("Comparação efeito do motor elétrico e rodas de reação em wz")
    xlabel("Tempo [s]");
    ylabel("Rotatão instantânea em z [rad/s] ");
    plot(t,wz,'r');
    plot(t,wz_nat,'black');
    legend(['Motores elétricos ligados';'Motores elétricos desligados']);
else
    disp("O valor análise não é um valor válido (Digite 1 ou 2).")
end