function [dx,Fer]=matematico_clase(t,x)
%% aqui esta el de eusebia funcionando
M = 0.38; %kg
I = 0.020803; % kg/m2
l = 0.25;
lc = l/2;
%% ACTUALIZAR VARIABLES DE ESTADO
q1=x(1);
dq1 = x(2);
int_error = x(3);
%% Constantes de sintonizacion
kp = 2.5;
kd = 0.5;
ki = 0.17;
%% Calculos
%angulo = 30;
%ang_rad = angulo*pi/180;
x_d = l*cos(0.1*2*pi*t);
y_d = l*sin(0.1*2*pi*t);
qd = atan(y_d/x_d);
error = qd - q1;
derror = - dq1;
Taw = kp*error + ki*int_error + kd*derror;
%%
ddq = (Taw - M*lc*sin(q1))/((M*lc^2+I));
%% espacio de estados
dx=zeros(3,1);
dx(1) = dq1; % la derivada de la pose es velocidad
dx(2) = ddq; % kla derivada de la velicdad es la acelearcion
dx(3) = error; % la dewrivada de x3 es el error
x = l*cos(q1);
y = l*sin(q1);
Fer = [qd x y];
tf=40; % segundos
x0 = [0; % posicion inicial
0;% velocidad inicial
0];
[T,XX]=ode23(@matematico_clase,[0 tf],x0);
[~,Fer] = cellfun(@(t,x) matematico_clase(t,x.'), num2cell(T), num2cell(XX,2),'uni',0);
FE = cell2mat(Fer);
figure(1)
plot(T,XX(:,1), "-", T, FE(:,1), "--", 'LineWidth',2.5); %, T, FE(:,2), ,"--",
xlabel('\textbf{Time $(s)$}','FontSize',15,'FontWeight','bold','Interpreter','latex');
ylabel('\textbf{Position en $\theta $}','FontSize',15,'FontWeight','bold','Interpreter','latex');
grid on;
hold on;
legend ('$\theta$', '$\theta_d$','Interpreter','latex')
figure(2)
plot(FE(:,2),FE(:,3), "-", 'LineWidth',2.5); %, T, FE(:,2), ,"--",
xlabel('\textbf{X $(m)$}','FontSize',15,'FontWeight','bold','Interpreter','latex');
ylabel('\textbf{Y $(m) $}','FontSize',15,'FontWeight','bold','Interpreter','latex');
grid on;
hold on;
legend ('$\theta$', '$\theta_d$','Interpreter','latex')
tf=10; % segundos
x0 = [0;0;% primera art pose vel q1
0;0;%segunda art pose vel q2
0;0];%integral eq1 eq2
[T,XX]=ode23(@matematico_claseII,[0 tf],x0);
[~,Fer] = cellfun(@(t,x) matematico_claseII(t,x.'), num2cell(T), num2cell(XX,2),'uni',0);
FE = cell2mat(Fer);
figure(1)
plot(T,XX(:,1), "-", T, FE(:,1), "--", 'LineWidth',2.5); %, T, FE(:,2), ,"--",
xlabel('\textbf{Time $(s)$}','FontSize',15,'FontWeight','bold','Interpreter','latex');
ylabel('\textbf{Position en $\theta_1 $}','FontSize',15,'FontWeight','bold','Interpreter','latex');
grid on;
hold on;
legend ('$\theta_1$', '$\theta_{1d}$','Interpreter','latex')
figure(2)
plot(T,XX(:,3), "-", T, FE(:,2), "--", 'LineWidth',2.5); %, T, FE(:,2), ,"--",
xlabel('\textbf{Time $(s)$}','FontSize',15,'FontWeight','bold','Interpreter','latex');
ylabel('\textbf{Position en $\theta_2 $}','FontSize',15,'FontWeight','bold','Interpreter','latex');
grid on;
hold on;
legend ('$\theta_2$', '$\theta_{2d}$','Interpreter','latex')
figure(3)
plot(T, FE(:,5), "-", T, FE(:,6), "-", 'LineWidth',2.5); %, T, FE(:,2), ,"--",
xlabel('\textbf{Time $(s)$}','FontSize',15,'FontWeight','bold','Interpreter','latex');
ylabel('\textbf{Control signal $\tau $}','FontSize',15,'FontWeight','bold','Interpreter','latex');
grid on;
hold on;
legend ('$\tau_1$', '$\tau_2$','Interpreter','latex')
figure(4)
plot(FE(:,7), FE(:,8), "-", 'LineWidth',2.5); %, T, FE(:,2), ,"--",
xlabel('\textbf{ $x(m)$}','FontSize',15,'FontWeight','bold','Interpreter','latex');
ylabel('\textbf{ $y (m) $}','FontSize',15,'FontWeight','bold','Interpreter','latex');
grid on;
hold on;
legend ('$r$','Interpreter','latex')
function [dx,Fer]=matematico_claseII(t,x)
%% ACTUALIZAR VARIABLES DE ESTADO
q1=x(1);
dq1 = x(2);
q2 = x(3);
dq2 = x(4);
ieq1 = x(5);
ieq2 = x(6);
dq = [dq1;dq2];
g = 9.81;
m1 = 3.8;
m2 = 3.8 ;
l1 = .25;
l2 = .25;
lc1 = .125;
lc2 = .125;
I1 = 0.0802;
I2 = 0.0802;
%% Matrices
M = [(m1*lc1^2+m2*lc2^2+m2*l1^2 + 2*m2*l1*lc2*cos(q2)+I1+I2),
(m2*lc2^2+m2*l1*lc2*cos(q2)+I2);
(m2*lc2^2+m2*l1*lc2*cos(q2)+I2),(m2*lc2^2+I2)];
C = [-2*m2*l1*lc2*sin(q2)*dq2, -2*l1*lc2*sin(q2)*dq2;
-2*l1*lc2*sin(q2)*dq1, 0];
G = [m1*g*lc1*sin(q1) + m2*g*l1*sin(q1) + m2*g*lc2*sin(q1 + q2);
m2*g*lc2*sin(q1 + q2)];
F = [2.88*dq1;0.175*dq2];
xd = 30;
yd = 1;
l1 = 38.46;
l2 = 1.82;
%% calculo de valores deseados
q2_d = acos((xd^2 + yd^2 - l1^2 - l2^2 )/(2*l1*l2));
q1_d = atan(yd/xd)- atan((l2*sin(q2_d))/(l1+l2*cos(q2_d)));
%% Constantes de sintonizacion
kp_1 = 40;
kd_1 = 10;
ki_1 = 80;
kp_2 = 40;
kd_2 = 10;
ki_2 = 10;
%% Calculos
%angulo = 30;
%ang_rad = angulo*pi/180;
qd_1 = 10*pi/180;
qd_2 = 10*pi/180;
eq1 = qd_1 - q1;
deq1 = - dq1;
eq2 = qd_2 - q2;
deq2 = - dq2;
Taw_1 = kp_1*eq1 + kd_1*deq1 + ki_1*ieq1;
Taw_2 = kp_2*eq2 + kd_2*deq2+ ki_2*ieq2;
taw = [Taw_1;
Taw_2];
%%
ddq = inv(M)*(taw- C*dq-G-F);
%% espacio de estados
dx=zeros(6,1);
dx(1) = dq1; % la derivada de la pose es velocidad
dx(2) = ddq(1); % kla derivada de la velicdad es la acelearcion
dx(3) = dq2;
dx(4) = ddq(2);
dx(5) = eq1;
dx(6) = eq2;
x = l1*cos(q1)+l2*cos(q1+q2);
y = l1*sin(q1)+l2*sin(q1+q2);
%261
Fer = [qd_1 qd_2 eq1 eq2 Taw_1 Taw_2 x y];
