clear()

m1 = 0.44
m2 = 0.3
m3 = 0.18
m4 = 0.765
p = 0.03
c = 0.01
g = 9.8
x0 = 0
xponto0 = 0
y0 = 0
yponto0 = 0
z0 = 0
zponto0 = 0
passos = 0
total_passos = 5
pas = 1000 + 200*total_passos
t1 = zeros(1:pas)
t2 = zeros(1:pas)
t3 = zeros(1:pas)
t4 = zeros(1:pas)
x = zeros(1:pas)
xponto = zeros(1:pas)
y = zeros(1:pas)
yponto = zeros(1:pas)
t = linspace(0,1,pas)//para 10s de simulação
i = int(1);
txi = 0
tyi = 0

while passos <= total_passos
    i = 1
    while i <= pas 
        if 0 + 200*(passos) < i && i <= 200*(1 + passos) then
            t1(i) = 0.00;
            t2(i) = 0.01*g;
            t3(i) = 0.01*g;
            t4(i) = 0.00;
           
        elseif  (200*(1 + passos)< i) && (i <= 400 + 200*passos) then   
            t1(i) = 0.04*g;
            t2(i) = 0.00;
            t3(i) = 0.00;
            t4(i) = 0.00; 
           
        elseif  (400 + 200*passos < i) && (i <= 600 + 200*passos) then
            t1(i) = 0.00;
            t2(i) = -0.01*g;
            t3(i) = -0.01*g;
            t4(i) = 0.00; 
           
        elseif (600 + 200*passos < i) && (i <= 800 + 200*passos) then
            t1(i) = -0.04*g;
            t2(i) = 0.00;
            t3(i) = 0.00;
            t4(i) = 0.00;
       
        elseif (820 + 200*passos < i) && (i <= 1000 + 200*passos) then
            t1(i) = 0.00;
            t2(i) = 0.00;
            t3(i) = 0.00;
            t4(i) = 0.01*g;     
        end
        i = i + 1
    end
    passos = passos + 5
end

A = [[0 0 0 1 0 0];[0 0 0 0 1 0];[0 0 0 0 0 1];[0 0 0 -2*c/(m1+m3) 0 0];[0 0 0 0 -2*c/(m1+m2+m3) 0];[0 0 0 0 0 -c/(m4+m3/2)]]
B = [[0];[0];[0];[2*%pi/(p*(m1+m3))];[2*%pi/(p*(m1+m2+3*m3))];[2*%pi/(p*(m4+m3/2))]]

ut = zeros(6,pas)
ut(4,:) = t1
ut(5,:) = (t2+t3)
ut(6,:) = t4


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 = [[0];[0];[0];[0];[0];[0]]

sys_ss = syslin('c', A, B, C ,D);

// Polinômio característico
P_c = poly(A,"roots");

// Avaliação d comportamento no domínio da frequência
// Obtenção das funções de transferência

sys_tf = ss2tf(sys_ss);
disp(sys_tf)
   
//Avaliação gráfica dos polos do sistema
f9 = scf(9)
plzr(sys_ss)

//Diagramas de bode
f10 = scf(10)
bode(sys_ss)


//Obtenção dos valores de polos

//disp(spec(A))

//Obtenção das frequências naturais e dos coeficientes de amortecimento

//[wn,dam] = damp(sys_tf)


x0e = zeros(6,1)

[y,x] = csim ("impuls", t, sys_ss, x0e); 

X = x(1,:);
Vx = x(2,:);
Y = x(3,:);
Vy = x(4,:);
Z = x(5,:);
Vz = x(6,:);

f11=scf(11);
subplot(231)
plot(t,X);
title("Deslocamento eixo x","fontsize", 3);
xlabel("tempo[s]","fontsize",3);
ylabel("deslocamento[mm]","fontsize",3);

subplot(232)
plot(t,Vx);
title("Velocidade no eixo x","fontsize", 3);
xlabel("tempo[s]","fontsize", 3);
ylabel("velocidade[mm/s]","fontsize",3);

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

subplot(234)
plot(t,Vy);
title("Velocidade no eixo y","fontsize", 3);
xlabel("tempo[s]","fontsize", 3);
ylabel("velocidade[mm/s]","fontsize",3);

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


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

[y,x] = csim("step", t, sys_ss, x0e); 

X = x(1,:);
Vx = x(2,:);
Y = x(3,:);
Vy = x(4,:);
Z = x(5,:);
Vz = x(6,:);


f12 = scf(12)
subplot(231)
plot(t,X);
title("Deslocamento eixo x","fontsize", 3);
xlabel("tempo[s]","fontsize",3);
ylabel("deslocamento[mm]","fontsize",3);

subplot(232)
plot(t,Vx);
title("Velocidade no eixo x","fontsize", 3);
xlabel("tempo[s]","fontsize", 3);
ylabel("velocidade[mm/s]","fontsize",3);

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

subplot(234)
plot(t,Vy);
title("Velocidade no eixo y","fontsize", 3);
xlabel("tempo[s]","fontsize", 3);
ylabel("velocidade[mm/s]","fontsize",3);

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


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