clear
l=0.241;
Ce=0.116
ro=1.225
r=0.1145
b=Ce*4*ro*(r^4)/(%pi^2)
g=9.81;
mc=0.338
mr=0.039
m=mc+4*mr
Iy=4.9523*10^-3
w0 = 1.00*sqrt(m*g/(4*b))
passo=1000

t0=0; // intervalo de tempo
tf=10; // Instante final de simulação
t=linspace(t0,tf,passo);

//condições inciais
Xo=[1;-10;0;0;0;0];

A =[0 0 1 0 0 0; 0 0 0 1 0 0; 0 0 0 0 -g 0;0 0 0 0 0 0;0 0 0 0 0 1;0 0 0 0 0 0];
B = [0 0 0 0;0 0 0 0;0 0 0 0;-b/m -b/m -b/m -b/m; 0 0 0 0;0 (-b*l)/Iy 0 (b*l)/Iy];
C = [1 0 0 0 0 0;0 1 0 0 0 0; 0 0 1 0 0 0; 0 0 0 1 0 0;0 0 0 0 1 0;0 0 0 0 0 1];
D = zeros(6,4);
sistema = syslin('c', A,B,C,D);
s = poly(0,"s");
G = syslin('c',C*inv(s*eye(A)-A)*B + D);
disp(G);

T0=1 //definição de quando começa a variar a velocidade 
variacao = 0.001
for i=1:passo
    if t(i) > T0 then
        /*CASO 1
          u(1,i)=w0^2
          u(2,i)=(w0*(1+0.1*delta))^2
          u(3,i)=w0^2
          u(4,i)=(w0*(1-0.1*delta))^2
          CASO 2
          u(1,i)=w0^2
          u(2,i)=(w0*(1-0.1*delta))^2
          u(3,i)=w0^2
          u(4,i)=(w0*(1+0.1*delta))^2
          CASO 3
          u(1,i)=w0^2
          u(2,i)=(w0*(1+0.1*delta))^2
          u(3,i)=w0^2
          u(4,i)=(w0*(1+0.1*delta))^2
          CAS0 4 
          u(1,i)=w0^2
          u(2,i)=(w0*(1-0.1*delta))^2
          u(3,i)=w0^2
          u(4,i)=(w0*(1-0.1*delta))^2*/
        u(1,i)=(w0)^2-w0^2
        u(2,i)=(w0*(1-0.1*variacao))^2-w0^2
        u(3,i)=(w0)^2-w0^2
        u(4,i)=(w0*(1+0.1*variacao))^2-w0^2
        
    else
        u(1,i)=w0^2-w0^2
        u(2,i)=w0^2-w0^2
        u(3,i)=w0^2-w0^2
        u(4,i)=w0^2-w0^2
    end
end

Y=csim(u,t,sistema,Xo)

scf(0)
plot(t,[Y(1,:);Y(2,:)])
xgrid(2)
xtitle("Posição do drone ao longo do tempo ","tempo[s]","espaço[m]")
legend("x", "z");

a.x_label.font_size = 4;
a.y_label.font_size = 4;
a.title.font_size = 4;
a.font_size = 4;
a.legend.font_size = 4;

scf(1)

plot(t,[Y(3,:);Y(4,:)])
legend('u','w')
xgrid(2)
xtitle("Velocidade linear do drone longo do tempo ","tempo[s]","Velocidade[m/s]")


a.x_label.font_size = 4;
a.y_label.font_size = 4;
a.title.font_size = 4;
a.font_size = 4;
a.legend.font_size = 4;

scf(2)

plot(t,[Y(5,:)])
xgrid(2)
xtitle("Posição angular theta ao longo do tempo ","tempo[s]","Espaço[rad]")


a.x_label.font_size = 4;
a.y_label.font_size = 4;
a.title.font_size = 4;
a.font_size = 4;
a.legend.font_size = 4;


scf(3)
plot(t,Y(6,:))
xgrid(2)
xtitle("Velocidade angular (q)","tempo[s]","Velocidade[rad/s]")

a.x_label.font_size = 4;
a.y_label.font_size = 4;
a.title.font_size = 4;
a.font_size = 4;
a.legend.font_size = 4;

//Diagramas de Bode
scf(4)
bode(G(1,2))

scf(5)
bode(G(1,4))
scf(6)
bode(G(2,1))
scf(7)
bode(G(3,2))
scf(8)
bode(G(3,4))
scf(9)
bode(G(4,1))
scf(10)
bode(G(5,2))
scf(11)
bode(G(5,4))
scf(12)
bode(G(6,2))
scf(13)
bode(G(6,4))