clc; clear all; close all;
format short g

ti=0; h=0.001; tf = 5; t=ti:h:tf;  %vector tiempo
ci=[0; 0; 0; 0;0;0]; %condiciones iniciales
opciones=odeset('RelTol',1e-3,'InitialStep', 1e-3, 'MaxStep',1e-3);
[t,x]=ode45('cap8_robot2gdlPID',t,ci,opciones);
   
q1=x(:,1); q2=x(:,2);
qp1=x(:,3); qp2=x(:,4);
int1=x(:,3); int2=x(:,4);
    
   [n,m]=size(t);
tau1=zeros(n,1); tau2=zeros(n,1);
qtilde1=zeros(n,1); qtilde2=zeros(n,1);
xef=zeros(n,1); yef=zeros(n,1);
   
   beta1=0.15; beta2=0.15; l1=0.45; l2=0.45;
for k=1:n   
 [qt tau] = cap8_PIDrobot2gdl([q1(k); q2(k)], [qp1(k); qp2(k)],[int1(k); int2(k)]);
    tau1(k)=tau(1); tau2(k)=tau(2);
    qtilde1(k)=qt(1); qtilde2(k)=qt(2);
     [xef(k), yef(k),~ ]=cinematica_r2gdl(beta1, l1, q1(k), beta2,l2,q2(k));
       p=Rz(-pi/2)*[xef(k), yef(k), beta1+beta2]';
       xef(k)=p(1);
       yef(k)=p(2);
end
      
subplot(3,3,1); plot(t,qtilde1,t,qtilde2)
subplot(3,3,2); plot( t, tau1, t,tau2)
subplot(3,3,3); plot( t,int1,t,int2)
subplot(3,3,4); plot(xef,yef)
subplot(3,3,5); plot(qtilde1, qp1, qtilde2,qp2)
subplot(3,3,6); plot(t, qp1, t,qp2)