// Construction du TP grue

// Compilation et lien des routines en fortran
//unix("f77 -c grue.f");
if ~c_link('grue') then link(sci+'/tmp/grue/grue.o','grue');end

//unix("f77 -c regbn.f");
if ~c_link('regbn') then link(sci+'/tmp/grue/regbn.o','regbn');end

// Chargement des fonctions
getf(sci+'/tmp/grue/fun_grue','c');

// Grue. unites: m kg s
//
//
g=10;m=500;J=50;b=0.4;Cr=20;Mc=5000;Cd=500;
// donnees integration numerique 
/////////////////////////////
// 0 ---> deltat sec de (Dini,Rini)   (Dfin,Rfin)
deltat=10; Dini=0; Dfin=20 ; Rini=10 ; Rfin = 5 ;
// Regulateur sur D : gain Kd (1/s) et temps Td (s) ; 
    Kd= 1/0.01 ;           //Td=8/Kd; 
// Regulateur sur R : gain Kr (1/s) et temps Tr (s) ;
    Kr=  1/0.01  ;         //Tr=8/Kr;
//
ystr=[ 'deltat'; 'Dini'; 'Dfin' ; 'Rini' ; 'Rfin' ; ' Kd ' ; ' Kr' ];
num=2;
while num==2 ,
Xstart=evstr(x_mdialog(['Choix des parametres'],...
                 ystr,string([deltat;Dini;Dfin;Rini;Rfin;Kd;Kr])));
deltat=Xstart(1);
Dini=Xstart(2);Dfin=Xstart(3);
Rini=Xstart(4);Rfin=Xstart(5);
Kd=Xstart(6);Kr=Xstart(7);
///////////////////////////////
Td=8/Kd; Tr=8/Kr;
param=[-Kd*Mc Td -Kr*(J/b+m*b) Tr];
//
// calcul de aa(1:5)
aa=inv([.....
1       1       1       1       1
5       6       7       8       9
5*4     6*5     7*6     8*7     9*8
5*4*3   6*5*4   7*6*5   8*7*6   9*8*7
5*4*3*2 6*5*4*3 7*6*5*4 8*7*6*5 9*8*7*6
           ]) * [1 0 0 0 0]' ;
//
//
// echelle de temps donne par la periode des petites oscillations
periode=2*%pi*sqrt((Rini+Rfin)/2/10); 
ti=0 ; nbstep=floor(deltat*3/(periode/10)); tf=nbstep*periode/10;
t = ti:tf/nbstep:tf;
//
// reponse  a variation quelconque de R et D. 
//  voir le source de la  function grue_dyn.m
//
// variable qp: positions generalisees q et impulsion generalises p
//
//       d    r    theta   P_d      P_r      P_theta
qpi=[ Dini    Rini     0       0        0       0       ]' ;
etat = ode(qpi,0,t,0.001,0.001,dyn1);etat=etat';
// visualisation des resultats 
//
// les echelles; 
xmin=%inf;xmax=-%inf;
ymin=%inf;
nnn=maxi(size(t));
x=[];z=[];Dc=[];Rc=[];
for i=1:nnn
  xx=etat(i,1)+etat(i,2)*sin(etat(i,3));
  xmin = mini(xmin,xx-0.25); xmax=maxi(xmax,xx+0.25);
  xmin=mini(xmin,etat(i,1)-0.5);xmax=maxi(xmax,etat(i,1)+0.5); 
  zz=-etat(i,2)*cos(etat(i,3));
  ymin = mini(ymin,zz-0.5);
  [D R]=dr1( (i-1)*(tf-ti)/nbstep );
  Dc=[Dc D]; Rc=[Rc R];
  x=[x xx]; z=[z zz];
end; 
xmin = xmin-0.1 ;
xmax = xmax+0.1;
ymin = ymin-0.1; 
ymax = 0.5 + 0.1;
pause;

xset("default");xbasc([0,1,2,3]);xclear();
//
   newone=['stop ';'encore!  '];
    xset("window",0);xselect();
    xsetech([0,0,1,1],[xmin,ymin,xmax,ymax])
    isoview(xmin,xmax,ymin,ymax);
      xpoly([xmin;xmax],[-0.05;-0.05],'lines');
     plot_grue(etat(1,1),etat(1,2),etat(1,3));
      xpoly(x,z,'lines');  
      plot_grue(Dfin,Rfin,0);pause;
      xset('alufunction',6)
   for i=2:nnn-1
      plot_grue(etat(i,1),etat(i,2),etat(i,3));xpause(800); 
      plot_grue(etat(i,1),etat(i,2),etat(i,3));
   end;
    xset('alufunction',3);
    pause;
    xset("window",1);xselect();
    plot2d([t',t'],[etat(:,1),Dc'],[-1,-2],'121','d@v1');xtitle('d et v1','sec','m')
    pause;
    xset("window",2);xselect();
    plot2d([t',t'],[etat(:,2),Rc'],[-1,-2],'121','r@v2');xtitle('r et  v2','sec','m')
    pause
    xset("window",3);xselect();
    plot2d(t',etat(:,1)-x');xtitle('Deviation horizontale (m) ')
    pause;
    xbasc([0,1,2,3]);xset("default");
   num=x_choose(newone,' Et maintenant ? ');
   if num==0|num==1 then break;end
end




  


