clear
clc
xdel(winsid())
// Massa do quadrotor
mt=0.76105;

// Momentos de inercia
Ix=0.2;
Iy=0.2;
Iz=0.03;

// Gravidade
g=-9.81;

// Areas frontal (Ax) e lateral
Ax=0.008553265;
Ay=Ax;
Az=0.0131505375;

// Coeficientes de arrasto
Cdx=0.13
Cdy=Cdx;
Cdz=0.27;

// Densidade do ar
ro=1.2041;

// Forcas
Fsus=7.4659005 // Forca de sustentacao
Fmax=8.277 // Forca maxima

// Tempo
t0 = 0;
tfinal = 5;
tpasso = 0.1;

t = [t0:tpasso:tfinal];

//---------------------------------------------

// Matriz A

A= [0 0 0 1 0 0 0 0 0 0 0 0;
    0 0 0 0 1 0 0 0 0 0 0 0;
    0 0 0 0 0 1 0 0 0 0 0 0;
    0 0 0 0 0 0 0 -g 0 0 0 0;
    0 0 0 0 0 0 0 0 0 0 0 0;
    0 0 0 0 0 0 0 0 0 0 0 0;
    0 0 0 0 0 0 0 0 0 1 0 0;
    0 0 0 0 0 0 0 0 0 0 1 0;
    0 0 0 0 0 0 0 0 0 0 0 1;
    0 0 0 0 0 0 0 0 0 0 0 0;
    0 0 0 0 0 0 0 0 0 0 0 0;
    0 0 0 0 0 0 0 0 0 0 0 0]

// Matriz B


B=[
0 0 0 0 0;
0 0 0 0 0;
0 0 0 0 0;
0 0 0 0 0;
0 0 0 0 0;
0 0 0 1/mt g;
0 0 0 0 0;
0 0 0 0 0;
0 0 0 0 0;
0 0 0 0 0;
0 1/Iy 0 0 0;
0 0 0 0 0;
]

// Matriz C

C = [1 0 0 0 0 0 0 0 0 0 0 0;
     0 1 0 0 0 0 0 0 0 0 0 0;
     0 0 1 0 0 0 0 0 0 0 0 0;
     0 0 0 1 0 0 0 0 0 0 0 0;
     0 0 0 0 1 0 0 0 0 0 0 0;
     0 0 0 0 0 1 0 0 0 0 0 0;
     0 0 0 0 0 0 1 0 0 0 0 0;
     0 0 0 0 0 0 0 1 0 0 0 0;
     0 0 0 0 0 0 0 0 1 0 0 0;
     0 0 0 0 0 0 0 0 0 0 0 0;
     0 0 0 0 0 0 0 0 0 0 0 0;
     0 0 0 0 0 0 0 0 0 0 0 0]

// Matriz D

D=[0 0 0 0 0;
   0 0 0 0 0;
   0 0 0 0 0;
   0 0 0 0 0;
   0 0 0 0 0;
   0 0 0 0 0;
   0 0 0 0 0;
   0 0 0 0 0;
   0 0 0 0 0;
   0 0 0 0 0;
   0 0 0 0 0;
   0 0 0 0 0;]

// Sistema linear
sistema = syslin('c',A,B,C,D);

//-------------------------------

// Funcoes de transferencia
G1=ss2tf(sistema);

// Mostra as funcoes de transferencia no terminal
disp(G1)

// Definicao de s
s = poly(0, 's');

// Remover o comentario da funcao desejada para obter os diagramas

// Entrada Ttheta
h = syslin('c', (49.5-(7.105e-15)*s -(2.665e-15)*(s^2)-1.776e-15*(s^3))/(s^4)); // Saida x
//h = syslin('c', (49.5-(3.553e-15)*s -(8.882e-16)*(s^2))/(s^3)); // Saida u
//h = syslin('c', (5)/(s^2)); // Saida theta

// Entrada F
//h = syslin('c', (1.3139741-(2.220e-16)*s)/(s^2)); // Saida z
//h = syslin('c', (1.3139741)/(s)); // Saida w

// Diagrama de polos
f10=scf(1);
plzr(h)

// Diagrama de bode
f10=scf(2);
clf(); bode(h, 0.01, 1e20);

//-------------------------------

// Entradas
// Torque em phi
u1=zeros (t);
u1(:)=0;

// Torque em theta
u2=zeros (t);
u2(:)=0.01;

// Torque em psi
u3=zeros (t);
u3(:)=0;

// Forca
u4=zeros (t);
u4(:)=Fmax;

// Entrada unitaria
u5=zeros (t);
u5(:)=1;

// Matriz U
U = [u1; u2; u3; u4; u5];

// Condicoes iniciais
x0e = [0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0]; // Possivel condicao em theta (x0e(8)): 15*%pi/180


// Simulacao
[y,x] = csim (U, t, sistema, x0e); 

//-------------------------------

// Variaveis para o plot
X = x(1,:);
Y = x(2,:);
Z = x(3,:);
u = x(4,:);
v = x(5,:);
w = x(6,:);
phi = x(7,:);
theta = x(8,:);
psi = x(9,:);
p = x(10,:);
q = x(11,:);
r = x(12,:);

// Plot dos gráficos de deslocamento e velocidade
f10=scf(3);
subplot(231)
plot(t,X);
title("Deslocamento eixo x","fontsize", 3);
xlabel("tempo[s]","fontsize",3);
ylabel("deslocamento[m]","fontsize",3);

subplot(232)
plot(t,Y);
title("Deslocamento no eixo y","fontsize", 3);
xlabel("tempo[s]","fontsize", 3);
ylabel("deslocamento[m]","fontsize",3);

subplot(233)
plot(t,Z);
title("Deslocamento no eixo z","fontsize", 3);
xlabel("tempo[s]","fontsize", 3);
ylabel("deslocamento[m]","fontsize",3);

subplot(234)
plot(t,u);
title("Velocidade no eixo x","fontsize", 3);
xlabel("tempo[s]","fontsize", 3);
ylabel("velocidade[m/s]","fontsize",3);

subplot(235)
plot(t,v);
title("Velocidade no eixo y","fontsize", 3);
xlabel("tempo[s]","fontsize", 3);
ylabel("velocidade[m/s]","fontsize",3);

subplot(236)
plot(t,y(6,:));
title("Velocidade no eixo z","fontsize", 3);
xlabel("tempo[s]","fontsize", 3);
ylabel("velocidade[m/s]","fontsize",3);

// Plot dos gráficos de deslocamento e velocidade angular
f10=scf(4);
subplot(231)
plot(t,phi);
title("Deslocamento angular no eixo x","fontsize", 3);
xlabel("tempo[s]","fontsize",3);
ylabel("${\phi}[rad]$","fontsize",4);

subplot(232)
plot(t,theta);
title("Deslocamento angular no eixo y","fontsize", 3);
xlabel("tempo[s]","fontsize", 3);
ylabel("${\theta}[rad]$","fontsize",4);

subplot(233)
plot(t,psi);
title("Deslocamento angular no eixo z","fontsize", 3);
xlabel("tempo[s]","fontsize", 3);
ylabel("${\psi}[rad]$","fontsize",4);

subplot(234)
plot(t,p);
title("Velocidade angular no eixo x","fontsize", 3);
xlabel("tempo[s]","fontsize", 3);
ylabel("$\dot{\varphi}[rad/s]$","fontsize",4);

subplot(235)
plot(t,q);
title("Velocidade angular no eixo y","fontsize", 3);
xlabel("tempo[s]","fontsize", 3);
ylabel("$\dot{\theta}[rad/s]$","fontsize",4);

subplot(236)
plot(t,r);
title("Velocidade angular no eixo z","fontsize", 3);
xlabel("tempo[s]","fontsize", 3);
ylabel("$\dot{\psi}[rad/s]$","fontsize",4);