//TP Grue 3eme partie

m=500;//kg masse de la charge
M=5000;//kg masse du chariot
g=10;// m/s2 accélération de la gravité
J= 50;//kgm2 inertie du treuil
rho=0.4;//m rayon du treuil
G1= 20;//kg/s constante de frottement du chariot
G2=20;//kgm/s constante de frottement du treuil 

T=9// s durée du déplacement
Ri=5;//m longueur initiale du cable
Rf=3;//m longueur finale du cable
xi=0;//m position initiale du chariot
xf=15;//m position finale du chariot

J1=J/(m*rho^2);
J2=J1+1;
M1=M/m;
m1=m*rho;

function [z]=mu(y),  z=J2*M1+J1*sin(y(5))^2, endfunction
function [z]=alpha(y), z=y(3)*y(6)^2 + g*cos(y(5)), endfunction

//Trajectoire de reference de la charge
function z=xiref(t) 
  z=xi +(xf-xi)*(t/T).^5 .*(...
      126-420*(t/T)+540*(t/T).^2-315*(t/T).^3+70*(t/ T).^4),
endfunction
function z=zetaref(t),
  z=-Ri-(Rf-Ri)*((xiref(t) -xi)/(xf-xi)).*...
(9-12*((xiref(t) -xi)/(xf-xi))+4*((xiref(t) -xi)/(xf-xi)).^2) ,
endfunction


//Vitesse de reference de la charge
function z=xidotref(t)
  z=((xf-xi)/T)*(t/T).^4 .*(5*126-6*420*(t/T)+7*540*(t/T).^2-8*315*(t/T).^3+9* ...
			 70*(t/T).^4), 
endfunction
function z=zetadotref(t),
  z=-((Rf-Ri)/(xf-xi))*(9-2*12*((xiref(t) -xi)/(xf-xi))+...
3*4*((xiref(t) -xi)/(xf-xi)).^2).*xidotref(t),
endfunction

//Acceleration de reference de la charge
function z=xiddotref(t),
z=((xf-xi)/(T^2))*(t/T).^3 .*(4*5*126-5*6*420*(t/T)+6*7*540*(t/T).^2-7*8*315*(t/T).^3+8*9* ...
			 70*(t/T).^4), 
endfunction
function z=zetaddotref(t),
  z=-((Rf-Ri)/(xf-xi)^2)*(-2*12+2*3*4*((xiref(t) -xi)/(xf-xi)))...
.*xidotref(t) -((Rf-Ri)/(xf-xi))*(9-2*12*((xiref(t) -xi)/(xf-xi))+...
3*4*((xiref(t) -xi)/(xf-xi)).^2).*xiddotref(t),
endfunction

//Derivee troisieme de reference de la charge
function z=xidddotref(t),
z=((xf-xi)/(T^3))*(t/T).^2 .*(3*4*5*126-4*5*6*420*(t/T)+5*6*7*540*(t/T).^2-6*7*8*315*(t/T).^3+7*8*9* ...
			 70*(t/T).^4), 
endfunction
function z=zetadddotref(t),
  z=-((Rf-Ri)/(xf-xi)^3)*2*3*4*xidotref(t)...
-2*((Rf-Ri)/(xf-xi)^2)*(-2*12+2*3*4*((xiref(t) -xi)/(xf-xi)))...
.*xiddotref(t) -((Rf-Ri)/(xf-xi))*(9-2*12*((xiref(t) -xi)/(xf-xi))+...
3*4*((xiref(t) -xi)/(xf-xi)).^2).*xidddotref(t),
endfunction

//Trajectoire de reference du chariot
function z=xref(t),
  z=xiref(t) - xiddotref(t).*zetaref(t)./(zetaddotref(t)+g),
endfunction

//Trajectoire de reference de la longueur de cable
function z=Rref(t),
  z=sqrt(zetaref(t).^2 + (xiddotref(t).*zetaref(t)./(zetaddotref(t)+g)).^2),
endfunction

function z=Q(t),
z=((xidddotref(t).*zetaref(t)+xiddotref(t).*zetadotref(t)).*(zetaddotref(t)+g)...
- xiddotref(t).*zetaref(t).*zetadddotref(t))./((zetaddotref(t)+g).^2),
endfunction

//vitesse de reference du chariot
function z=xdotref(t),
  z=xidotref(t) - Q(t),
endfunction

//Vitesse de reference de la longueur du cable
function z=Rdotref(t),
  z=(zetaref(t).*zetadotref(t) + Q(t).*xiddotref(t).*zetaref(t)./(zetaddotref(t)+g))./Rref(t),
endfunction

//constantes de temps du systeme boucle
ex=0.05//s constante de temps /x
eR=0.05//s constante de temps /R
K1=5// gain proportionnel /x
K3=5// gain proportionnel /R
K5=0// inutilise
K6=0// inutilise

function z=FBF(t,y),
  z=-(M/ex)*((y(2)-xdotref(t))+K1*(y(1)-xref(t))) + M*(K5*y(5)+K6*y(6)),
endfunction

function z=CBF(t,y),
  z=m*g*rho +(((J/rho)+m*rho)/eR)*((y(4)-Rdotref(t)) + K3*(y(3)-Rref(t))),
endfunction

// Equations du système
// y =[x xdot,R,Rdot,theta,thetadot] 
function ydot=Grue(t,y)
  ydot=ones(y);
  ydot(1)=y(2)
  ydot(2)=(1/mu(y))*(J1*alpha(y)*sin(y(5)) + ...
		     (1/m1)*(CBF(t,y)+G2*y(4))*sin(y(5)) +...
		     (J2/m)*(FBF(t,y)-G1*y(2)))
  ydot(3)=y(4)
  ydot(4)=(1/mu(y))*(M1*alpha(y) - ...
		     ((M1+sin(y(5))^2)/m1)*(CBF(t,y)+G2*y(4)) - (1/m)* ...
		     (FBF(t,y)-G1*y(2))*sin(y(5))) 
  ydot(5)=y(6)
  ydot(6)= -(1/(y(3)*mu(y)))*(J1*alpha(y)*sin(y(5))+...
			     (1/m1)*(CBF(t,y)+G2*y(4))*sin(y(5))...  
			     +(J2/m)*(FBF(t,y)-G1*y(2)))*cos(y(5)) ...
	   -(1/y(3))*(2*y(4)*y(6)+g*sin(y(5))) 
endfunction

stacksize(8000000);
y0=[xi,0,Ri,0,0,0]' ; t=0:1.e-4:T;
y=ode(y0,0,t,Grue) ;

xset('window',1);
xbasc();
plot2d([t',t'],[y(1,:);(y(1,:)+y(3,:).*sin(y(5,:)))]')
legends(['position chariot','position horizontale de la charge'],[1,2],2);
xtitle(' ','temps (s)','position (m)')

xset('window',2);
xbasc();
plot2d([t',t'],[y(3,:);(-y(3,:).*cos(y(5,:)))]')
legends(['longueur du cable','position verticale de la charge'],[1,2],2);
xtitle(' ','temps (s)','position (m)')

xset('window',3);
xbasc();
plot((y(1,:)+y(3,:).*sin(y(5,:)))',(-y(3,:).*cos(y(5,:)))')
xtitle('trajectoire de la charge ','position horizontale (m)',...
       'position verticale (m)')

xset('window',4);
xbasc();
plot2d(t,(-(M/ex)*((y(2,:)-xdotref(t))+K1*(y(1,:)-xref(t))) + M*(K5*y(5,:)+K6*y(6,:))))
xtitle(' ','temps (s)','force appliquee au chariot (N)')

xset('window',5);
xbasc();
plot2d(t,10*(m*g*rho +(((J/rho)+m*rho)/eR)*((y(4,:)+Rdotref(t)) + K3*(y(3,:)-Rref(t)))))
xtitle(' ','temps (s)','couple applique au treuil (Nm)')

xset('window',6);

function grueplot(y) 
  pt1=[y(1),0];
  pt2=[y(1)+y(3)*sin(y(5)),-y(3)*cos(y(5))] ;
  xset("dashes",5)
  xpoly([pt1(1),pt2(1)],[pt1(2),pt2(2)],"lines")
  w= 1;h=0.25; xfrect(pt1(1)-w/2,pt1(2)+h/2,w,h);
  w= 0.25;h=0.25; xfrect(pt2(1)-w/2,pt2(2)+h/2,w,h);
endfunction

function grueAnim(y,t,step) 
  dr=driver();
  if driver()=='Rec' then driver('X11'),end //unset memorisation
  kp=xget("pixmap");xset("pixmap",1);
  plot2d([],[],-1,"030"," ",[0,-6,15,1])
  [my,ny]=size(y);
  xx=(y(1,1:step:ny)+y(3,1:step:ny).*sin(y(5,1:step:ny)))';
  yy=(-y(3,1:step:ny).*cos(y(5,1:step:ny)))';
  xx1=xiref(t);
  yy1=zetaref(t);
  for i=1:step:ny 
    xset("wwpc");  //erase
    grueplot(y(:,i)) 
    plot2d(xx,yy,1,"000"); 
    plot2d(xx1,yy1,3,"000");
    xset("wshow");  //show
  end
  plot2d(xx,yy,1,"000"); 
  plot2d(xx1,yy1,3,"000"); 
  xset("wshow");  //show
  xset("pixmap",kp);driver(dr)
endfunction

//halt()
xselect()   
grueAnim(y,t,5000);