%PENDULO FURURA
%ecuaciones de estado no lineales
%x1punto=x3
%x2punto=x4
%x3punto=(c^2*cos(x1)*x3^2*sin(x1)-2*c*cos(x1)^2*e*x3*x4*sin(x1)-a*x4^2*sin
%(x1)*cos(x1)*b-d*sin(x1)*b-sin(x1)*a*x4^2*cos(x1)*e-sin(x1)*d*e
%+sin(x1)*cos(x1)^3*a*x4^2*e+sin(x1)*cos(x1)^2*d*e+c*cos(x1)*u)/(-a*b+c^2*cos(x1)^2-a*e+a*e*cos(x1)^2)
%x4punto=(-a*c*x3^2*sin(x1)+2*a*e*x3*x4*sin(x1)*cos(x1)+c*cos(x1)*d*sin(x1)
%-u*a+c*cos(x1)^2*a*x4^2*sin(x1))/(-a*b+c^2*cos(x1)^2-a*e+a*e*cos(x1)^2)
%x3punto y x4 punto se obtuvieron solucionando el sistema de 2*2 de las
%ecuaciones de movimiento:
clear
clc
s=solve('a*(x-(x4)^2*sin(x1)*cos(x1))+c*y*cos(x1)-d*sin(x1)=0','c*x*cos(x1)-c*(x3)^2*sin(x1)+2*e*x3*x4*sin(x1)*cos(x1)+(b+e*sin(x1)^2)*y=u','x','y');
%LINEALIZACION:
%calculo de los coeficientes de las matrices A y B
syms x1 x2 x3 x4 u a b c d e
A(1,:)=jacobian(x3,[x1,x2,x3,x4]);
A(2,:)=jacobian(x4,[x1,x2,x3,x4]);
A(3,:)=jacobian(s.x,[x1,x2,x3,x4]);
A(4,:)=jacobian(s.y,[x1,x2,x3,x4]);
B(1,:)=jacobian(x3,u);
B(2,:)=jacobian(x4,u);
B(3,:)=jacobian(s.x,u);
B(4,:)=jacobian(s.y,u);
%en el punto de equibibrio
x1=0;
x2=0;
x3=0;
x4=0;
A=subs(A)
B=subs(B)%substituir los simbolos por sus respectivos valores
%valores parametros dados:
l=0.413; %metros- longitud pendulo
M=0.01; %kg- masa del peso
Jp=0.0009; %Kgm^2- inercia del brazo
r=0.235; %m- longitud del brazo
J=0.05; %Kgm^2- inercia del pendulo
m=0.02; %kg- masa del brazo
g=9.81; %m/s^2-gravedad
%definicion parametros ecuaciones de movimiento pendulo Furuta
a=Jp+M*l^2;
b=J+M*r^2+m*r^2;
c=M*r*l;
d=(M+m/2)*g*l;
e=Jp+m*l^2;
disp('reemplazando los parametros tenemos:')
A=subs(A),B=subs(B)
C=[1 0 0 0];%salida igual a posicion angular pendulo
disp('modelo en espacio de estados:')
sys=ss(A,B,C,0)