%DINAMICA SISTEMA: TWO-LINK MANIPULATOR
%autor:John Caipa
%programa para encontrar las ecuaciones de estado dadas
%por la dinamica del sistema y
%realizar la linealizacion alrededor de punto de equilibrio
%----------------------------------------------------
%ECUACIONES DE ESTADO
clear
clc
syms x1 x2 x3 x4 m11 m12 m22 k11 k12 u1 u2
f1=x3;
f2=x4;
s=solve('m11*f3+m12*cos(x1-x2)*f4+m12*sin(x1-x2)*x4^2+k11*sin(x1)=u1','m12*cos(x1-x2)*f3+m22*f4-m12*sin(x1-x2)*x3^2+k12*sin(x2)=u2','f3','f4');
f3=s.f3;
f4=s.f4;
%LINEALIZACION
A(1,:)=jacobian(f1,[x1,x2,x3,x4]);
A(2,:)=jacobian(f2,[x1,x2,x3,x4]);
A(3,:)=jacobian(f3,[x1,x2,x3,x4]);
A(4,:)=jacobian(f4,[x1,x2,x3,x4]);
B(1,1)=jacobian(f1,u1);
B(2,1)=jacobian(f2,u1);
B(3,1)=jacobian(f3,u1);
B(4,1)=jacobian(f4,u1);
B(1,2)=jacobian(f1,u2);
B(2,2)=jacobian(f2,u2);
B(3,2)=jacobian(f3,u2);
B(4,2)=jacobian(f4,u2);
%-----------------------------------------------------------------------
%en el punto de equilibrio posicion vertical arriba para los dos brazos
x1=0;
x2=0;
x3=0;
x4=0;
A=subs(A);
B=subs(B);%substituir los simbolos por sus respectivos valores
%valores parametros dados:
m1=0.5;
m2=0.5;
l1=0.5;
lc1=0.25;
lc2=0.25;
I1=0.083;
I2=0.033;
g=9.81;
m11=m1*lc1^2+m2*l1^2+I1;
m12=m2*l1*lc2;
m22=m2*lc2^2+I2;
k11=-(m1*lc1+m2*l1)*g;
k12=-m2*g*lc2;
disp(' REMPLAZANDO LOS PARAMETROS TENEMOS:')
disp('1. punto de equilibrio posicion vertical arriba')
A=subs(A),B=subs(B)
C=[1 0 0 0;0 1 0 0];
D=zeros(2,2);
Xi=[0 0 0 0]
%valores propios
disp('valores propios:')
d=eig(A)
%analisis controlabilidad
disp('La matriz de controlabilidad es:')
Co=ctrb(A,B)
disp('El rango de la matriz de controlabilidad es:')
rank(Co)
%analisis observabilidad
disp('La matriz de observabilidad es:')
Ob=obsv(A,C)
disp('El rango de la matriz de observabilidad es:')
rank(Ob)
