clc; clear all; close all; 
format short g
global tau1 tau2
ti=0; h=0.001; tf = 5; t=ti:h:tf;  %vector tiempo
ci=[0; 0; 0; 0];  %condiciones iniciales
opciones=odeset('RelTol',1e-3, 'InitialStep',1e-3, 'MaxStep',1e-3);
%solución numérica del robot de 2 gdl
disp('Advertencia: la simulación puede durar varios segundos......')
disp('Espere por favor..............')
[t,x]=ode45('cap7_iderobot2gdl',t,ci,opciones);
   
q1=x(:,1); q2=x(:,2); %vector de posición 
qp1=x(:,3); qp2=x(:,4); %vector de velocidad
    
[m n]=size(t); 
Int1=zeros(m,1); 
Int2=zeros(m,1); 
Int3=zeros(m,1); 
Int4=zeros(m,1); 
Int5=zeros(m,1); 
Int6=zeros(m,1); 
Int7=zeros(m,1); 
u1=zeros(m,1);
u2=zeros(m,1);
    
 for k=1:m  %registro de la señal del par aplicado tau
           xp=cap7_iderobot2gdl(t(k),[x(k,1),x(k,2),x(k,3), x(k,4)]);
           u1(k,1) =tau1;
           u2(k,1) =tau2;
 end

 for k=2:m %integrales del regresor
     Int1(k,1)=Int1(k-1,1)+h*(qp1(k,1)*u1(k,1)+qp2(k,1)*u2(k,1));
     Int2(k,1)=Int2(k-1,1)+h*qp1(k,1)*qp1(k,1);
     Int3(k,1)=Int3(k-1,1)+h*qp2(k,1)*qp2(k,1);
     Int4(k,1)=Int4(k-1,1)+h*abs(qp1(k,1));
     Int5(k,1)=Int5(k-1,1)+h*abs(qp2(k,1));
     Int6(k,1)=Int6(k-1,1)+h*qp1(k,1)*(1-abs(sign(qp1(k,1))));
     Int7(k,1)=Int7(k-1,1)+h*qp2(k,1)*(1-abs(sign(qp2(k,1))));
 end

%regresor psi
 fi11=(1/2)*qp1.*qp1; 
 fi12=cos(q2).*qp1.*(qp1+qp2);  
 fi13=qp2.*((1/2)*qp2+qp1);
 fi14=1-cos(q1);  
 fi15=1-cos(q1+q2); 
 fi16=Int2; 
 fi17=Int3;
 fi18=Int4;  
 fi19=Int5; 
 fi110=Int6; 
 fi111=Int7;
 
 fi=[fi11, fi12, fi13, fi14, fi15, fi16, fi17, fi18, fi19, fi110, fi111];
 [r,theta] =mincuad(Int1,fi);
    theta