//TP Grue

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=10// 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 (position chariot approx. position horizontale charge)
function z=xref(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

//Vitesse de la trajectoire de reference (vitesse chariot approx. vitesse horizontale charge)
function z=xdotref(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

// Longueur du cable de reference
function z=Rref(t),
  z=((Rf-Ri)/(xf-xi))*(xref(t) -xi) +Ri;
endfunction
// Hauteur charge de reference (approx. -longueur cable)
function z=zref(t),
  z=-Rref(t);
endfunction

//Vitesse de reference du cable 
function z=Rdotref(t),
  z=((Rf-Ri)/(xf-xi))*xdotref(t),
endfunction

//constantes de temps du systeme boucle
ex=0.005//s
eR=0.005//s
K1=50
K3=50//
K5=0
K6=0//

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=xref(t);
  yy1=zref(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,1500);