// deuxième partie (Voir feuille) 
//-------------------------------

function y =yref(t)
  y =x0+(x1-x0)*(t/T)^5 .*(126-420*(t/T)+540*(t/T)^2-315*(t/T)^3+70*(t/ ...
						  T)^4)
endfunction

function y =yrefdot(t)	 
  y =630*((x1-x0)/T)*(t/T)^4 .*(1-(t/T))^4
endfunction

function y =yrefd2dot(t)	 
  y =2520*((x1-x0)/(T^2))*(t/T)^3 .*(1-5*(t/T)+9*(t/T)^2-7*(t/T)^3+2*(t/ ...
						  T)^4)
endfunction

function y =yrefd3dot(t)	
  y =2520*((x1-x0)/(T^3))*(t/T)^2 .*(3-20*(t/T)+45*(t/T)^2-42*(t/T)^3+14* ...
				   (t/T)^4)
endfunction

function y =yrefd4dot(t)	 
  y =15120*((x1-x0)/(T^4))*(t/T) .*(1-10*(t/T)+30*(t/T)^2-35*(t/T)^3+14*(t/ ...
						  T)^4)
endfunction

function y = Fref(t)
  y = (M+m)* yrefd2dot(t)+(M+m)*(r/k)*yrefd3dot(t)+((M*m)/k)*yrefd4dot(t)
  I=find(t > T);
  y(I) = 0;
endfunction

function y = xref(t)
  y = yref(t)+(r/k)*yrefdot(t)+(m/k)*yrefd2dot(t)
  I=find(t > T);
  y(I) = x1;
endfunction

function y = xrefdot(t)
  y = yrefdot(t)+(r/k)*yrefd2dot(t)+(m/k)*yrefd3dot(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(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)');