clc; clear all;
 close all;
format short
%parámetros de simulación:
ti=0; h=0.0025; tf = 5; ts=ti:h:tf;  %vector tiempo 
opciones=odeset('RelTol',1e-3,'InitialStep', 2.5e-3,'MaxStep',2.5e-3);
ci=[0; 0; 0; 0;0;0];
[t,x]=ode45('cap8_robot3gdl',ts,ci,opciones);
q1=x(:,1); q2=x(:,2); q3=x(:,3); %posiciones articulares
qp1=x(:,4); qp2=x(:,5); qp3=x(:,6); %velocidades articulares
   [n,m]=size(t);
tau1=zeros(n,1); tau2=zeros(n,1); tau3=zeros(n,1);
  
qtilde1=zeros(n,1); qtilde2=zeros(n,1); qtilde3=zeros(n,1);
xef=zeros(n,1); yef=zeros(n,1); zef=zeros(n,1);
   
beta1=0.15; beta2=0.15; beta3=0.15; l1=1; l2=0.45; l3=0.45;
  for k=1:n  
    [qt tau] = cap8_PDrobot3gdl([q1(k); q2(k); q3(k)],[qp1(k);qp2(k); qp3(k)]);
    tau1(k)=tau(1); tau2(k)=tau(2); tau3(k)=tau(3);
    qtilde1(k)=qt(1); qtilde2(k)=qt(2); qtilde3(k)=qt(3);
   [xef(k), yef(k),zef(k)]=cinematica_r3gdl (beta1, l1, q1(k), beta2, l2,q2(k),beta3,l3,q3(k));       
  end
  
      
subplot(2,2,1); plot(t,qtilde1,t,qtilde2, t,qtilde3)
subplot(2,2,2); plot( t, tau1, t,tau2, t,tau3)
subplot(2,2,3); plot3(xef,yef,zef)
subplot(2,2,4); plot(qtilde1, qp1, qtilde2,qp2, qtilde3,qp3)