%  Mouse-driven motion planning
%      for  the doucble pendulum at the "musée des sciences de la Villette"
%  mai 99
%  Ecole des Mines de Paris, 
%  Centre Automatique et Systemes
%  60 Bd St Michel 
%  75272 Paris
%    Nicolas Petit: petit@cas.ensmp.fr
%    Pierre Rouchon: rouchon@cas.ensmp.fr
%    Yves Lenoir : lenoir@cas.ensmp.fr
% 
% Linear approximation 
% 2 homogeneous beams of length l 
% vertical deviation th1 and th2
% troley abscissa  D  is the control 
%  Motion planning based on the flat output: 
%         z= D + (35/36)*th1*l + (7/12)*l*th2 is the flat output
% g gravity
%
%  th1= z^(2)/g  - (1/3)*(z^(4)/g^2)*l
%  th2= z^(2)/g  + (1/9)*(z^(4)/g^2)*l
%  D  = z - (14/9)*z^(2)/g  * l  + (7/27)*(z^(4)/g^2)*(l^2)
%
%      
%
% 
%     
clear all;

L=.5;  % beam length 
g=9.81; % gravity in  m/s^2
scale=10.8825;

A = 0.75; th1= 18*(pi/180); th2= (18-27)*(pi/180);

D=A; z= D + (35/36)*th1*L + (7/12)*L*th2; dz=0.;
d2z= g*(3*th2+th1)/4;
d3z=0;
d4z=(9/4)*(g^2/L)*(th2-th1);



dtmin=0.001;
dtmax=1.;

dt=0.02;
n=floor(10/dt);
Dr=D*ones(1,n);th1r=th1*ones(1,n);th2r=th2*ones(1,n);zr=z*ones(1,n);

nn=[1:n-1];
tr=0*Dr;

figure(1),clf;
set(gca,'position',[0 0 1 1],'visible','off','Xlim',[0 19],'Ylim',[0 14]....
   ,'nextplot','add');


%  trolley
Xt=1+[0   1  1  0  ] + scale*D;
Yt=1+[0   0  1/2  1/2  ];
trol=fill(Xt,Yt,'g','EraseMode','background');

%  ref
Xr=1+[0   1  0.5  0  ] + scale*D;
Yr=0.3+[0   0  1/2  0  ];
ref=fill(Xr,Yr,'y','EraseMode','background');

% pendule 1 et 2 avec les deux articulations
Xp1=1.5 + scale*[ D  D+L*sin(th1)   ];
Yp1=1.5+scale*[0  L*cos(th1)   ];
Xp2=1.5 + scale*[   D+L*sin(th1)  D+L*sin(th1)+L*sin(th2) ];
Yp2=1.5+scale*[ L*cos(th1)  L*cos(th1)+L*cos(th2) ];

Xj1=1.5 + scale*(D + L*cos(0:(2*pi/10):(2*pi))/25 );
Yj1=1.5+scale*(L*sin(0:(2*pi/10):(2*pi))/25);
Xj2=1.5 + scale*(D+L*sin(th1) + L*cos(0:(2*pi/10):(2*pi))/25 );
Yj2=1.5+scale*(L*cos(th1) + L*sin(0:(2*pi/10):(2*pi))/25);

P1=line(Xp1,Yp1,'EraseMode','background','LineWidth', 4, 'Color', [1 0 0 ]);
P2=line(Xp2,Yp2,'EraseMode','background','LineWidth', 4, 'Color', [0 0 1 ]);


J1=fill(Xj1,Yj1,'k','EraseMode','background','Facecolor',[1 0 0 ]);
J2=fill(Xj2,Yj2,'k','EraseMode','background','Facecolor',[0 0 1 ]);

set(gcf,'WindowButtonMotionFcn','1;');



% title
text(9.5,13.5,'DOUBLE INVERTED PENDULUM: MOUSE-DRIVEN MOTION PLANNING','color',[0 0 0],...
   'EraseMode','background','HorizontalAlignment','center');
text(9.5,13,'display speed slider','color',[0 0 0],'EraseMode','background','HorizontalAlignment','center');
p1=[7.5/19 12.6/14 4/19 0.2/14];
Hslider=uicontrol(figure(1),'units','normalized','style','slider','position',p1,...
			'min',log(dtmin),'max',log(dtmax),'value',log(dt),...
         'callback',['buf=get(Hslider,''value'');dt=exp(buf);']); 
      









i=0;

t1=0.3;
s=1.3;

seuil=0.1;


t2=t1/s;
t3=t2/s;
t4=t3/s;
t5=t4/s;





F=[
  -1/t1 1/t1  0     0     0     
   0    -1/t2 1/t2  0     0      
   0     0   -1/t3  1/t3  0      
   0     0     0   -1/t4 1/t4    
   0     0     0     0  -1/t5 
   ];
   F2=F*F;
F3=F2*F;
F4=F3*F;
M=[ 1 0 0 0 0 
   F(1,:)
   F2(1,:)
   F3(1,:)
   F4(1,:)
];

N=inv(M)*[z;dz;d2z;d3z;d4z];
z1=N(1);
z2=N(2);
z3=N(3);
z4=N(4);
z5=N(5);



Hplay=uicontrol('style','push',...
                'units','normalized','position',[15/19 12.7/14 2/19 .5/14], ....
                'string','Plot','callback',[ ...
                'figure(2);clf;' ...
                'subplot(211);plot(tr,zr); zoom on;' ...
                'title(''flat output  (m) versus time (s)'');' ...
                'subplot(212);plot(tr,Dr);zoom on;'...
                'title(''troley position (m) versus time (s)'');'...
                'figure(3);clf;plot(tr,th1r,''-'',tr,th2r,'':'');zoom on;' ...
                'title(''angle 1 (solid line) and angle 2 (dashed line) (rd) versus time (s)'');' ...
                'pause;figure(1);' ...
                ]);
            
                           
set(Hplay,'visible','off');
             
Hrun=uicontrol('style','push',...
                'units','normalized','position',[1/19 12.7/14 2/19 .5/14], ....
                'string','stop','callback', ...
                ' i=2;' );
set(Hrun,'visible','off');

figure(1);
pause(1)
set(Hplay,'visible','on')
set(Hrun,'visible','on')
Tini=2;
tt=0;
while (i < 1)
   tt=tt+dt;
   if  (tt > Tini)
     buf=get(gca,'CurrentPoint');
     A=(buf(1,1)-1.5)/scale;
     A=max(0,A);
     A=min(1.4,A); 
   end;
   z1=(z1+dt*z2/t1)/(1+dt/t1);
   z2=(z2+dt*z3/t2)/(1+dt/t2);
   z3=(z3+dt*z4/t3)/(1+dt/t3);
   z4=(z4+dt*z5/t4)/(1+dt/t4);
   t5b=t5;
   sat=A-z5;
   if(tt > Tini)
     t5b=t5*max(1,abs(sat)/seuil);
   end;
   z5=(z5+dt*A/t5b)/(1+dt/t5b);
 
   N=M*[z1;z2;z3;z4;z5];
   z=N(1);
   dz=N(2);
   d2z=N(3);
   d3z=N(4);
   d4z=N(5);
   th1= d2z/g  - (1/3)*(d4z/g^2)*L;
   th2= d2z/g  + (1/9)*(d4z/g^2)*L;
   D  = z - (14/9)*d2z/g  * L  + (7/27)*(d4z/g^2)*(L^2);
   
   Xt=1+[0   1  1  0  ] + scale*D;
   Xr=1+[0   1  0.5  0  ] + scale*A;

   Xp1=1.5 + scale*[ D  D+L*sin(th1)   ];
   Yp1=1.5+scale*[0  L*cos(th1)   ];
   Xp2=1.5 + scale*[   D+L*sin(th1)  D+L*sin(th1)+L*sin(th2) ];
   Yp2=1.5+scale*[ L*cos(th1)  L*cos(th1)+L*cos(th2) ];

   Xj1=1.5 + scale*(D + L*cos(0:(2*pi/10):(2*pi))/25 );
   Yj1=1.5+scale*(L*sin(0:(2*pi/10):(2*pi))/25);
   Xj2=1.5 + scale*(D+L*sin(th1) + L*cos(0:(2*pi/10):(2*pi))/25 );
   Yj2=1.5+scale*(L*cos(th1) + L*sin(0:(2*pi/10):(2*pi))/25);
   
   set(trol, 'XData',Xt); 
   set(ref, 'XData',Xr); 
   set(P1,'XData',Xp1,'Ydata',Yp1);
   set(J1,'XData',Xj1,'Ydata',Yj1); 
   set(P2,'XData',Xp2,'Ydata',Yp2); 
   set(J2,'XData',Xj2,'Ydata',Yj2); 
   drawnow;
   Dr(nn)=Dr(nn+1); Dr(n)=D;
   zr(nn)=zr(nn+1); zr(n)=z;
   th1r(nn)=th1r(nn+1); th1r(n)=th1;
   th2r(nn)=th2r(nn+1); th2r(n)=th2;
   tr(nn)=tr(nn+1);tr(n)=tt;
   

  
end;


close(figure(1));
close(figure(2));
close(figure(3));


return