%  Mouse-driven motion planning
%      for  heavy chain system
%  march 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
% 
% Linear approximation (Cf the model studied by Daniel Bernoulli 1738 )
%     D_tt H  =  D_x (  g x D_x H ) = 0 
%     H(x=L,t)= D (the control, horizontal position of the trolley) 
%
% flat output: y(t)=H(0,t) the position of the free end 
% motion planing formulae 
% 
%  H(x,t) = 1/(2 pi) int_0^pi [ y(t - 2 sqrt(x/g) sin s) + y(t + 2 sqrt(x/g) sin s) ] ds
%   derived via  symbolic calculus  (Poisson integral representation of the 
%   Bessel function J_0) 
% 
%     
clear all;  
L=1.;  % rope  length in  m
A_max= 1.5*L ; % amplitude of horizontal motion  depth  in  m
g=9.81; % garvity in  m/s^2
T_max=2*sqrt(A_max/g);  % interval of motion (s)
DT=2*sqrt(L/g);
scale=min(11., 11/max(A_max*14/19,L)); 

% space mesh 
dx=L/40.;
x=[0:dx:L];
nx=length(x);
H=zeros(1,nx);
% time step 
dt0=DT/100;
dt=dt0/4/2; 
dtmin=DT/1000;
dtmax=DT/3/2;

YY=0*[-DT:dt0:DT];
mt=length(YY);
m0=(mt+1)/2.;
mm=[1:mt-1];

Y=0*[-5*DT:dt:0]; D=Y;t=Y;
nt=length(Y);
nn=[1:nt-1];





figure(1),clf;
%set(gcf,'renderer','openGL');

go=1;
mouse=0;
set(gca,'position',[0 0 1 1],'visible','off','Xlim',[0 19],'Ylim',[0 14]....
   ,'nextplot','add')
set(gcf,'CloseRequestFcn','go=0; closereq;',...
	'WindowButtonUpFcn',['mouse=0;set(ref,''facecolor'',''y'') ;'...
		'set(gcf,''WindowButtonMotionFcn'','''');']);
          

   
%  trolley

Xt=1+[0   1  1  0  ]+scale*H(nx);
Yt=12+[0   0  1/2  1/2  ];
trol=fill(Xt,Yt,'g','EraseMode','background');


% cable

Xc=1.5 + scale*H;
Yc=12+scale*(x-1);
chain=line(Xc,Yc,'EraseMode','background','LineWidth', 4, 'Color', [0 0 1 ]);

%  ref
Xr=1+[0   1  0.5  0  ] + scale*H(nx);
Yr=0.3+[0   0  1/2  0  ];

ref=fill(Xr,Yr,'y', 'EraseMode','xor',...
        'ButtonDownFcn',['mouse=1; set(ref,''facecolor'',''r'');' ...
           'set(gcf,''WindowButtonMotionFcn'',''1;'')']);




Hplay=uicontrol('style','push',...
                'units','normalized','position',[15/19 12.6/14 2/19 .5/14], ....
                'string','Plot','callback', ...
                ['figure(2);clf;' ...
                 'subplot(212);' ...
                 'plot(t,Y,''linewidth'',3);'...
                 'title(''flat output  (m) versus time'');'...
                 'subplot(211);'...
                 'plot(t,D,''linewidth'',3);'...
                 'title(''troley position (m/s)versus time''); 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', ...
                ' ; ' ,'visible','off');
             
             
             
             
  % title
text(9.5,13.5,'HEAVY CHAIN: 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(dt0),...
         'callback',['buf=get(Hslider,''value'');dt=exp(buf);']); 
           
             
             
             

figure(1);
pause(1)
set(Hrun,'string','Stop','callback','go=0;');
set(Hplay,'visible','on')
set(Hrun,'visible','on')



t1=T_max/5/5;
t2=t1/2;
seuil=A_max/5/10;
y1=0;
y2=0;



dth=pi/100;
th=[0:dth:pi-dth];
Sth=sin(th);

tt=0.;
y1_old=y1;
buf(1,1)=1.5;
while (go >0)
    if mouse > 0 
       buf=get(gca,'CurrentPoint');
    end
    A=(buf(1,1)-1.5)/scale;
    A=max(0,A);
    A=min(A_max,A); 
    
   y1=(y1+dt*y2/t1)/(1+dt/t1);
   t2b=t2*max(1,abs(A-y2)/seuil);
   y2=(y2+dt*A/t2b)/(1+dt/t2b);
   
   tt=tt+dt;
   if (tt >= dt0);
      ii=floor(tt/dt0);
      dy1=(y1-y1_old)/tt;
      for iii=1:ii;
         YY(mm)=YY(mm+1);YY(mt)=y1_old+iii*dt0*dy1;
      end;
      y1_old=y1-dy1*(tt-ii*dt0);
      tt=0.;
   end;
    
    
    for ix=1:nx;
       tau=2*sqrt(x(ix)/g);
       s=tau*Sth;
       ns=floor((mt-m0)*s/DT);
       H(ix)=(dth/2/pi)*sum(YY(m0+ns)+YY(m0-ns));
    end   

    
    
    Y(nn)=Y(nn+1); Y(nt)=H(1);
    D(nn)=D(nn+1); D(nt)=H(nx);
    t(nn)=t(nn+1); t(nt)=t(nt)+dt;
 
    
    
    Xt=1+[0   1  1  0  ] + scale*H(nx);
    Xr=1+[0   1  0.5  0  ] + scale*A;
    Xc=1.5 + scale*H;
    set(trol, 'XData',Xt); 
    set(chain,'XData',Xc); 
    set(ref,'XData',Xr); 
    drawnow;
   
end;


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

return
