// Script para Scilab 6.1.1: https://www.scilab.org
// Práctico de Sistemas y Control
// --- Hoja de ejercicios número 4, ejercicio 3, parte d ---

// Limpieza
clear;           // Borrar variables
clc();           // Limpiar consola
close(winsid()); // Cerrar ventanas de figuras

// Parámetros del sistema
k = 0.166;  // N /cm
k1 = k;
k2 = k1;
k3 = k1*k2/(k1+k2);
b = 0.1;    // N s /cm 
r = 5;      // cm
a = 1;      // N/V
J = 2.293;  // N cm s^2 / rad
B = 1.605;  // N cm s / rad

// Representación en variables de estado
MA = [-(k1+k2)/b,        -k2*r/b,    0;..
               0,              0,    1;..
         -k2*r/J, -(k2+k3)*r^2/J, -B/J];
MB = [-a/b,   0;..
         0,   0;..
         0, 1/J];
MC = [0, 1, 0];
RVE = syslin('c', MA, MB, MC)

// Matriz de transferencia
MT = clean(ss2tf(RVE));

// Func. de transferencia desde la entrada V a la salida phi
FT_1_1 = MT(1,1);

// Func. de transferencia desde la entrada V a la salida phi (calculada)
s = poly(0,"s");
den = s^3 + (B*b+2*k1*J)/(J*b)*s^2 + (3/2*k1*r^2*b+2*k1*B)/(J*b)*s + ..
      (2*k1^2*r^2)/(J*b);
FT_1_1_calc = (k1*r*a)/(J*b)/den;

// Comparar funciones de transferencia
disp("Func. de transferencia:", FT_1_1)
disp("Func. de transferencia calculada:", FT_1_1_calc)

// Polos del sistema
polos = roots(den);
disp("Polos del sistema:", polos)

// Parámetros derivados
// polos(1) tiene parte imaginaria nula y polos(2) tiene parte imag. no nula
q = -polos(1);
alpha = abs(real(polos(2))); // Factor de amortiguameinto 
Wd = abs(imag(polos(2)));    // Frecuencia real
Wn = sqrt(alpha^2+Wd^2);     // Frecuencia natural
zeta = alpha/Wn;             // Relación de amortiguamiento

// Intervalo de tiempo a simular
t = linspace(0,10,1000);

// Estado inicial
phi0 = %pi/2;
x0 = [0; phi0; 0];

// Función de entrada
V0 = 2;
function u=entrada(t)
    u=[V0; 0]
endfunction

// Simulación
[phi, x] = csim(entrada, t, RVE, [0;phi0;0]);

// Respuesta calculada
c1 = (k*r*a/(J*b)*V0)/(q*(alpha^2+Wd^2));
c2 = (q*(2*k/b-q)*(B/J-q)*phi0-k*r*a/(J*b)*V0)/(q*((alpha-q)^2+Wd^2));
c3 = phi0-c1-c2;
c4 = (2*k/b*B/J*phi0-(2*q*alpha+alpha^2+Wd^2)*c1-(alpha^2+Wd^2)*c2)/q;
phi_calc = c1 + c2*exp(-q*t) + c3*exp(-alpha*t).*cos(Wd*t) ..
           + (c4-alpha*c3)/Wd*exp(-alpha*t).*sin(Wd*t)

// Error en norma infinito entre la respuesta simulada y calculada
err = max(abs(phi-phi_calc));
disp("Error en norma infinito entre resp. simulada y calculada:", err)

// Gráficas
subplot(311)
plot2d(t',x(1,:)')
xlabel("$t \ (\mathrm{s})$")
ylabel("$x (t) \ (\mathrm{cm})$")
title("Posición del punto sobre el que actúa el actuador")
subplot(312)
plot2d(t',[phi',phi_calc'],leg="Simulada@Calculada")
xlabel("$t \ (\mathrm{s})$")
ylabel("$\varphi (t) \ (\mathrm{rad})$")
title("Posición angular del disco")
subplot(313)
plot2d(t',x(3,:)')
xlabel("$t \ (\mathrm{s})$") 
ylabel("$\dot{\varphi} (t) \ (\frac{\mathrm{rad}}{\mathrm{s}})$")
title("Velocidad angular del disco")