clear all 
close all 
clc 

% Pendulo Invertido 
% Malha aberta
M = 10; m = 2; ell = 1; g = 9.8;
K = [10 50 -300 -100];
A = [0 1 0 0; 0 0 (m*g)/M 0; 0 0 0 1; 0 0 (M+m)*g/(M*ell) 0];
B = [0;1/M ;0; 1/(M*ell)];

phi0 = 85*pi/180;

% Malha fechada
Af = A+B*K;
x0 = [0 0 phi0-pi/2 0]';
D = 0;
tspan = [0:1e-3:20];

% Posicao
Cx = [1 0 0 0];
Fx = tf(ss(Af,x0,Cx,0))
xt = impulse(Fx,tspan);


% Angulo phi
Ct = [0 0 1 0];
Ft = tf(ss(Af,x0,Ct,0))
[th] = impulse(Ft,tspan);


xi0 = [0;0;phi0;0];
% 
% tfin = 20;
% sim('simu_pendulo_NL',[0 tfin])
% plot(xi1(:,1),xi1(:,2))


[t,xi] = ode45(@pendulo_naolinear,tspan,xi0); % time simulation
figure(1)
plot(tspan,xt,tspan,xi(:,1))

figure(2)
plot(tspan,(th+pi/2)*180/pi,tspan,xi(:,3)*180/pi)

function xidot = pendulo_naolinear(t,xi)
M = 10; m = 2; ell = 1; g = 9.8;
K = [10 50 -300 -100];

xi1 = xi(1);
xi2 = xi(2);
xi3 = xi(3);
xi4 = xi(4);
F = K*[xi1;xi2;xi3-pi/2;xi4];

auxdot = [M+m -m*ell*sin(xi3);-sin(xi3) ell]\[m*ell*cos(xi3)*xi4^2+F;-g*cos(xi3)];
xidot = [xi2;auxdot(1);xi4;auxdot(2)];
end