function y =yref(t)
  y = x0+(x1-x0)*(t/T)^7 .*(1716-9009*(t/T)+ 20020*(t/T)^2-24024*(t/T)^3+16380*(t/T)^4-6006*(t/T)^5+924*(t/T)^6)
endfunction


function y =yrefdot(t)	 
  y= ((x1-x0)/T)*(t/T)^6 .*(12012 -72072*(t/T) +180180*(t/T)^2 -240240*(t/T)^3 +180180*(t/T)^4 -72072*(t/T)^5 +12012*(t/T)^6)
endfunction

function y =yrefd2dot(t)	
  y= ((x1-x0)/(T^2))*(t/T)^5 .*(72072 -504504*(t/T) +1441440*(t/T)^2  -2162160*(t/T)^3 +1801800*(t/T)^4-  792792*(t/T)^5 +144144*(t/T)^6); 
endfunction

function y =yrefd3dot(t)	
  y= ((x1-x0)/(T^3))*(t/T)^4 .*(360360 -3027024*(t/T) +10090080*(t/T)^2 -17297280*(t/T)^3+16216200*(t/T)^4 -7927920*(t/T)^5 +1585584*(t/T)^6);
endfunction

function y =yrefd4dot(t)	 
  y= ((x1-x0)/(T^4))*(t/T)^3 .*(1441440 -15135120*(t/T) +60540480*(t/T)^2-121080960*(t/T)^3 +129729600*(t/T)^4 -71351280*(t/T)^5+15855840*(t/T)^6);
endfunction

function y =yrefd5dot(t)	 
  y= ((x1-x0)/(T^5))*(t/T)^2 .*(4324320 -60540480*(t/T) +302702400*(t/T)^2-726485760*(t/T)^3 +908107200*(t/T)^4 -570810240*(t/T)^5+142702560*(t/T)^6)
endfunction

function y =yrefd6dot(t)	
  y= ((x1-x0)/(T^6))*(t/T) .*(8648640 -181621440*(t/T) +1210809600*(t/T)^2-3632428800*(t/T)^3 +5448643200*(t/T)^4-3995671680*(t/T)^5 +1141620480*(t/T)^6);
endfunction

function y = Fref(t)
  y=(M+m1+m2)*yrefd2dot(t)+ ...
    (M+m1+m2)*(r1/k1+r2/k2)*yrefd3dot(t)+...
    (M*(m1/k1+ m2/k2+(r1*r2)/(k1*k2))+m1*(m2/k2+(r1*r2)/(k1*k2))+m2*(m1/k1+(r1*r2)/(k1*k2)))*yrefd4dot(t)+...
    (M*((m1*r2+m2*r1)/(k1*k2))+((m1*m2)/(k1*k2))*(r1+r2))*yrefd5dot(t)+...
    ((M*m1*m2)/(k1*k2))*yrefd6dot(t);
  I=find(t > T);
  y(I) = 0;
endfunction

function y = xref(t)
  y = yref(t)+((r1/k1)+(r2/k2))*yrefdot(t)+...
      ((m1/k1)+(m2/k2)+((r1*r2)/(k1*k2)))*yrefd2dot(t)+...
      ((m1*r2+m2*r1)/(k1*k2))*yrefd3dot(t)+...
      ((m1*m2)/(k1*k2))*yrefd4dot(t);
  I=find(t > T);
  y(I) = x1;
endfunction

function y = z1ref(t)
  y=yref(t)+((r1/k1)+(r2/k2))*yrefdot(t)+...
    ((m2/k2)+((r1*r2)/(k1* k2)))*yrefd2dot(t)+...
    ((r1* m2)/(k1*k2))*yrefd3dot(t); 
endfunction 

function y = z2ref(t)
  y= yref(t)+((r1/k1)+(r2/k2))*yrefdot(t)+ ...
     ((m1/k1)+((r1*r2)/(k1*k2)))*yrefd2dot(t)+...
     ((r2*m)/(k1*k2))*yrefd3dot(t);
endfunction 
  
function y = xrefdot(t) 
  y = yrefdot(t)+((r1/k1)+(r2/k2))*yrefd2dot(t)+ ...
      ((m1/k1)+(m2/k2)+((r1* r2)/(k1*k2)))*yrefd3dot(t)+...
      ((m1*r2+m2*r1)/(k1*k2))*yrefd4dot(t)+...
      ((m1*m2)/(k1*k2))*yrefd5dot(t);
  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)');