function y = Fref(t)
  y = M*420*((x1-x0)/(T^2))*(t/T)^2 .*(1-4*(t/T)+5*(t/T)^2-2*(t/T)^3); 
  I=find(t > T);
  y(I) = 0;
endfunction

function y = xref(t)
  y = x0+(x1-x0)*(t/T)^4 .*(35-84*(t/T)+70*(t/T)^2-20*(t/T)^3);
  I=find(t > T);
  y(I) = x1;
endfunction

function y = xrefdot(t)
  y = 140*((x1-x0)/T)*(t/T)^3 .*(1-(t/T))^3; 
  I=find(t > T);
  y(I) = 0; 
endfunction

function ydot = SmasseBF(t,y) 
// Smasse bouclée avec la commande F (3) 
// y = [ySmasse;e ] 
  FF= Fref(t) - kP*(y(1)-xref(t)) -kD*(y(2) - xrefdot(t)) - kI * y(7); 
  // 
  ydot = ones(y);
  ydot(1) = y(2) 
  ydot(2) = (1/M)*( FF -k1*(y(1)-y(3)) -r1 *(y(2)-y(4)) ...
		    -k2*(y(1)-y(5)) -r2 *(y(2)-y(6)))
  ydot(3) = y(4) 
  ydot(4) = (1/m1)*( k1 *(y(1)-y(3)) + r1 *(y(2)-y(4) ))
  ydot(5) = y(6) 
  ydot(6) = (1/m2)*( k2 *(y(1)-y(5)) + r2 *(y(2)-y(6) ))
  ydot(7) = y(1) - xref(t); 
endfunction

M=4 ; // masse moteur (kg)
m1=0.15 ; // masse transportŽe (kg)
m2=0.04 ; // seconde masse transportŽe (kg)
k1=m1*(4*2*%pi)^2 ; // raideur tige (N/m)
k2=m2*(7*2*%pi)^2 ; // raideur seconde tige (N/m)
r1=2*0.01*sqrt(k1*m1) ; // coefficient d'amortissement tige (Ns/m)
r2=2*0.012*sqrt(k2*m2) ; // coefficient d'amortissement seconde tige (Ns/m)
x0=0 ; // position intiale moteur (m)
x1=0.1; // position finale moteur (m)

txt = ['duree deplacement (s)';
       'gain proportionnel force moteur (N/m)';
       'gain derive force moteur (Ns/m)';
       'gain integral force moteur (N/ms)'];

if exists('T') then ST=string(T); else ST='0.5'; end 
if exists('kP') then SkP=string(kP); else SkP='0'; end 
if exists('kD') then SkD=string(kD); else SkD='0'; end 
if exists('kI') then SkI=string(kI); else SkI='0'; end 
v_init = [ ST;SkP;SkD;SkI];
sig=x_mdialog('Entrez les paramètres du controleur',txt,v_init);
if sig<>[] then 
  Sig = evstr(sig); 
  T = Sig(1); kP=Sig(2);kD=Sig(3);kI=Sig(4);
end

X=[x0;0;x0;0;x0;0;0];
pas = 1.e-4;
t=0:pas:2*T;

y = ode(X,0,t,SmasseBF);

// Calcul de la force 
yD = xref(t);
yDdot= xrefdot(t);
Force= Fref(t) -kP*(y(1,:)-yD) -kD*(y(2,:) - yDdot) -kI * y(7,:); 

xset('window',0);
xbasc();
plot2d(t,y(1,:)') 
xtitle('Position moteur','temps','m');

xset('window',1);
xbasc();
plot2d(t,(y(1,:)-yD)') 
xtitle('Ecart entre position moteur et réference','temps','m');

xset('window',2);
xbasc()
plot2d(t,y(3,:)') 
xtitle('Position de la premiere masse transportée','temps','m');

xset('window',3);
xbasc()
plot2d(t,y(5,:)') 
xtitle('Position de la deuxieme masse transportée','temps','m');

xset('window',4);
xbasc()
plot2d(t,(y(3,:)-y(1,:))') 
xtitle('Ecart premiere masse transportée, moteur','temps','m');

xset('window',5);
xbasc()
plot2d(t,(y(5,:)-y(1,:))') 
xtitle('Ecart deuxieme masse transportée, moteur','temps','m');

xset('window',6);
xbasc()
plot2d(t,Force') 
xtitle('Force produite par le moteur','temps','Force (N)');