%  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
%    Pierre Rouchon: rouchon@cas.ensmp.fr
% 
% Linear approximation 
% 1 homogeneous beams of length l 
% vertical deviation th
% troley abscissa  D  is the control 
%  Motion planning based on the flat output: 
%         z= D + (2/3)*th*l,  abscisse of the oscillation center,  is the flat output
% g gravity
%
%  th= z^(2)/g  
%  D  = z - (2/3)*z^(2)/g  * l 
%
%      
%
% 
%     
clear all;

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

A = 0.75; th= 0; 

D=A; 

z= D + (2/3)*th*L  ;


dtmin=0.001;
dtmax=1.;

dt=0.02;
n=floor(10/dt);
Dr=D*ones(1,n);thr=th*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
Xp=1.5 + scale*[ D  D+L*sin(th)   ];
Yp=1.5+scale*[0  L*cos(th)   ];

Xj=1.5 + scale*(D + L*cos(0:(2*pi/10):(2*pi))/50 );
Yj=1.5+scale*(L*sin(0:(2*pi/10):(2*pi))/50);

P=line(Xp,Yp,'EraseMode','background','LineWidth', 4, 'Color', [0 0 1 ]);
J=fill(Xj,Yj,'k','EraseMode','background','Facecolor',[0 0 1 ]);
set(gcf,'WindowButtonMotionFcn','1;');



% title
text(9.5,13.5,'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;

ts=0.25;

seuil=0.4;

z1=z;
z2=z;
z3=z;






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,thr,''-'');zoom on;' ...
                'title(''pendulum angle (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/ts)/(1+dt/ts);
   z2=(z2+dt*z3/ts)/(1+dt/ts);
   tb=ts;
   sat=A-z2;
   if(tt > Tini)
     tb=ts*max(1,abs(sat)/seuil);
   end;
   z3=(z3+dt*A/tb)/(1+dt/tb);
 
   z=z1;
   dz=(z2-z1)/ts; 
   dz2=(z3-z2)/ts;
   d2z=(dz2-dz)/ts;
   
   th= d2z/g ;
   D  = z - (2/3)*th  * L ;
   
   Xt=1+[0   1  1  0  ] + scale*D;
   Xr=1+[0   1  0.5  0  ] + scale*A;

   Xp=1.5 + scale*[ D  D+L*sin(th)   ];
   Yp=1.5+scale*[0  L*cos(th)   ];

   Xj=1.5 + scale*(D + L*cos(0:(2*pi/10):(2*pi))/50 );
   Yj=1.5+scale*(L*sin(0:(2*pi/10):(2*pi))/50);
   
   set(trol, 'XData',Xt); 
   set(ref, 'XData',Xr); 
   set(P,'XData',Xp,'Ydata',Yp);
   set(J,'XData',Xj,'Ydata',Yj); 
   drawnow;
   Dr(nn)=Dr(nn+1); Dr(n)=D;
   zr(nn)=zr(nn+1); zr(n)=z;
   thr(nn)=thr(nn+1); thr(n)=th;
    tr(nn)=tr(nn+1);tr(n)=tt;
   

  
end;


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


return