// Pb avec simple masse 

function ydot = Smasse(t,y) 
// y = [ x,xdot,z, zdot]
  ydot = ones(y);
  ydot(1) = y(2) 
  ydot(3) = y(4) 
  ydot(2) = (1/M)*( F(t) -k*(y(1)-y(3)) -r *(y(2)-y(4)))
  ydot(4) = (1/m)*( k *(y(1)-y(3)) + r *(y(2)-y(4) ))
endfunction

function y = xref(t) 
// trajectoire de ref sans tenir compte de la masse 
// pour x 
  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) 
// dérivée de xref 
  y = 140*((x1-x0)/T)*(t/T)^3 .*(1-(t/T))^3
  I=find(t > T);
  y(I) = 0;
endfunction

function y = Fref(t) 
// Force de référence 
  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 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(5); 
  // 
  ydot = ones(y);
  ydot(1) = y(2) 
  ydot(2) = (1/M)*( FF   -k*(y(1)-y(3)) -r *(y(2)-y(4)))
  ydot(3) = y(4) 
  ydot(4) = (1/m)*( k *(y(1)-y(3)) + r *(y(2)-y(4) ))
  ydot(5) = y(1) - xref(t); 
endfunction

M=4 ; // masse moteur (kg)
m=0.15 ; // masse transportée (kg)
k=m*(4*2*%pi)^2 ; // raideur tige (N/m)
r=2*0.01*sqrt(k*m) ; // coefficient d'amortissement 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;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(5,:); 

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 masse transportée','temps','m');

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

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