/* [wxMaxima batch file version 1] [ DO NOT EDIT BY HAND! ]*/
/* [ Created with wxMaxima version 12.04.0 ] */

/* [wxMaxima: comment start ]
Script permettant d'obtenir le codage des équations du vol du drone 
telles qu'elle ont été établies dans la présentation.

Attention il peut y avoir des erreurs :
 - dans le modèle ;
 - dans son codage.
Peu de tests ont été effectués... 

Le codage des expressions de Maxima est, mutatis mutandis, celui de Matlab/Octave.
   [wxMaxima: comment end   ] */

/* [wxMaxima: comment start ]
Liste des variables qui doivent rester des symboles & hypothèses sur leurs valeurs

À des détails près (omr = omega_r), les noms sont ceux de la présentation ;
la convention pour nommer les dérivées temporelles est : dx = dx/dt, d2x = d(dx)/dt ;
mais attention avec cette convention drx est la coordonnée de dr selon kx ; 
pas la dérivée temporelle de rx !
   [wxMaxima: comment end   ] */

/* [wxMaxima: input   start ] */
symbolise(x,y,z,omr,omp,omy,drx,dry,drz,dpx,dpy,dpz,dyx,dyy,dyz,
          dx,dy,dz,domr,domp,domy,ddrx,ddry,ddrz,ddpx,ddpy,ddpz,ddyx,ddyy,ddyz,
	  d2x,d2y,d2z,
	  m,I,iI,Irr,Irp,Iry,Irp,Ipp,Ipy,Iry,Ipy,Iyy,
	  Omega,D,X,dOmega,dD,C,F,Fd,
	  c,f,e,N,cfl,cfr,cbl,cbr,ffl,ffr,fbl,fbr,efl,efr,ebl,ebr,Nfl,Nfr,Nbl,Nbr,dzeta,
	  Phi,Res,a,k,kp,delta,rho,S,Cx,Sp,lambda,alpha,beta,gamma)$
assume(Phi>0,Res>0,a>0,k>0,kp>0,delta,0,rho>0,lambda>0,alpha>0,beta>0,gamma>0,Cx>0)$
/* [wxMaxima: input   end   ] */

/* [wxMaxima: comment start ]
Rel0 : Variables matricielles/vectorielles versus variables scalaires.

(Par rapport à la présentation on garde le signe + pour des cfl, cfr, cbl, cbr
et on met les met - dans l'expression de C)
   [wxMaxima: comment end   ] */

/* [wxMaxima: input   start ] */
Rel0:[Omega=matrix([0,omy,-omp],[-omy,0,omr],[omp,-omr,0]),
      omega=matrix([omr],[omp],[omy]),domega=matrix([domr],[domp],[domy]),
      D=matrix([drx,dry,drz],[dpx,dpy,dpz],[dyx,dyy,dyz]),
      dD=matrix([ddrx,ddry,ddrz],[ddpx,ddpy,ddpz],[ddyx,ddyy,ddyz]),
      X=matrix([x],[y],[z]),dX=matrix([dx],[dy],[dz]),d2X=matrix([d2x],[d2y],[d2z]),
/*      I=matrix([Irr,Irp,Iry],[Irp,Ipp,Ipy],[Iry,Ipy,Iyy]), */
      I=matrix([Irr,0,0],[0,Ipp,0],[0,0,Iyy]),
      C=matrix([a/2*(ffl+fbl-ffr-fbr)],[a/2*(ffl+ffr-fbl-fbr)],[cfl+cbr-cfr-cbl]),
      F=(ffl+ffr+fbl+fbr)*matrix([dyx],[dyy],[dyz])-matrix([0],[0],[m*g]),
      Fd=rho*S*Cx/2*sqrt(dx^2+dy^2+dz^2)*matrix([dx],[dy],[dz]),
      Cd=rho*lambda^3*Sp*Cx/4*sqrt(omr^2+omp^2+omy^2)*matrix([omr],[omp],[omy])
]$
Rel0:append(Rel0,[iI=invert(ev(I,Rel0))]);
/* [wxMaxima: input   end   ] */

/* [wxMaxima: comment start ]
Rel1 : couples et forces versus fréquences des moteurs
Rel2 : fréquence des moteurs versus tensions appliquées
Rel3 : variables de traînée versus vitesses linéaire et angulaires
   [wxMaxima: comment end   ] */

/* [wxMaxima: input   start ] */
Rel1:flatten(map(lambda([%u],block([c,f,N,e],[c,f,N,e]:map(lambda([%v],concat(%v,%u)),[c,f,N,e]),[c=2*%pi*Phi^2*(e/(2*%pi*Phi)-N)/Res,f=2*%pi*Phi^2*(e/(2*%pi*Phi)-N)*k/kp])),[fl,fr,bl,br]));
Rel2:map(lambda([%u],concat(N,%u)=-(Phi^2*%pi/(Res*kp)-dzeta/(2*delta))+sqrt((Phi^2*%pi/(Res*kp)-dzeta/(2*delta))^2+(Phi*concat(e,%u))/(Res*kp))),[fl,fr,bl,br]);
Rel3:[dzeta=dx*dyx+dy*dyy+dz*dyz,S=%pi*sqrt(dx^2*beta*gamma+dy^2*gamma*alpha+dz^2*alpha*beta)/sqrt(dx^2+dy^2+dz^2),Sp=%pi*(alpha*beta*gamma)^(2/3),lambda=(alpha*beta*gamma)^(1/3)/2];
/* [wxMaxima: input   end   ] */

/* [wxMaxima: comment start ]
EDOS0 : Équations dynamiques matricielles
   [wxMaxima: comment end   ] */

/* [wxMaxima: input   start ] */
EDOS0:[dD=Omega.D,domega=iI.(Omega.I.omega+C-Cd),d2X=(F-Fd)/m,dX=dX];
/* [wxMaxima: input   end   ] */

/* [wxMaxima: comment start ]
EDOS1 : Équations dynamiques scalaires
   [wxMaxima: comment end   ] */

/* [wxMaxima: input   start ] */
EDOS1:flatten(map(lambda([%u],block([%v:args(%u)[1],%w:args(%u)[2]],map("=",flatten(args(%v)),flatten(args(%w))))),ev(EDOS0,Rel0)));
/* [wxMaxima: input   end   ] */

/* [wxMaxima: comment start ]
Vars, SM : variables dynamiques et second membre ; d(Vars)/dt = SM
   [wxMaxima: comment end   ] */

/* [wxMaxima: input   start ] */
iRD:[[dx,x],[dy,y],[dz,z],[domr,omr],[domp,omp],[domy,omy],[ddrx,drx],[ddry,dry],[ddrz,drz],[ddpx,dpx],[ddpy,dpy],[ddpz,dpz],[ddyx,dyx],[ddyy,dyy],[ddyz,dyz],[d2x,dx],[d2y,dy],[d2z,dz]]$
Vars:map(lambda([%u],assoc(%u,iRD)),map(first,EDOS1));
SMs:map(second,EDOS1)$
SMs:ev(SMs,Rel1,eval,Rel2,eval,Rel3);
/* [wxMaxima: input   end   ] */

/* [wxMaxima: comment start ]
Données numériques
   [wxMaxima: comment end   ] */

/* [wxMaxima: input   start ] */
data0:[Mmot=5.0e-3,Mbat=17.0e-3,m=70.0e-3,a=8.5e-2,alpha=6.7e-2,beta=3.3e-2,gamma=1.8e-2,Res=1.0,Phi=0.5*0.01*0.004,rho=1.117,delta=2.0e-3,R=3.3e-2,Cx=0.5,g=9.81]$
data:[m=m,Phi=Phi,Res=Res,a=a,delta=delta,alpha=alpha,beta=beta,gamma=gamma,Cx=Cx,rho=rho,g=g,Irr=4*Mmot*(a/2)^2+(m-4*Mmot)*(beta^2+gamma^2)/5,Ipp=4*Mmot*(a/2)^2+(m-4*Mmot)*(gamma^2+alpha^2)/5,Iyy=4*Mmot*a^2/2+(m-4*Mmot)*(alpha^2+beta^2)/5,k=2*%pi*rho*delta^2*R^2,kp=rho*delta^3*R^2]$
data:map(lambda([%u],args(%u)[1]=float(ev(args(%u)[2],data0))),data);
/* [wxMaxima: input   end   ] */

/* [wxMaxima: comment start ]
Second Membre avec prise en compte des données numériques
en dehors des variables de Vars, il ne reste plus que les commandes : efl,efr,ebl,ebr dans SM
   [wxMaxima: comment end   ] */

/* [wxMaxima: input   start ] */
SMeff:float(ev(SMs,data));
/* [wxMaxima: input   end   ] */

/* [wxMaxima: comment start ]
Exemple de résolution avec rk : une ascension simple ; le tracé est celui de dz = dz/dt

(rk n'est pas le nec plus ultra en matière d'intégration d'EDO mais il a le mérite d'exister)
   [wxMaxima: comment end   ] */

/* [wxMaxima: input   start ] */
Vars0:ev(Vars,drx=1,dry=0,drz=0,dpx=0,dpy=1,dpz=0,dyx=0,dyy=0,dyz=1,omr=0,omp=0,omy=0,dx=0,dy=0,dz=0,x=0,y=0,z=0)$
COM:[efl=e,ebl=e,efr=e,ebr=e,eval,e=3.1]$
lX:rk(ev(SMeff,COM),Vars,Vars0,[t,0,5,0.01])$
plot2d([discrete,map(first,lX),map(lambda([%u],%u[16]),lX)]);
/* [wxMaxima: input   end   ] */

/* [wxMaxima: comment start ]
Et comme ce n'est pas très commode de se repérer dans le tableau lX : 
charger la fonction XvsT ci-dessous et exécuter la commande suivante.
   [wxMaxima: comment end   ] */

/* [wxMaxima: input   start ] */
EDO:[Vars,ev(SMeff,COM),[]]$
XvsT(EDO,z,dz)$
/* [wxMaxima: input   end   ] */

/* [wxMaxima: input   start ] */
XvsT(EDO,[l]):=block([Xs:EDO[1],Sms:EDO[2],Rel:EDO[3],N,lsub,lplot:[],leg:[legend],%w,lss,Ts:map(first,lX),lXf:[],lf:[],oklxf:0],
 map(lambda([%u],
   if (listp(%u) and first(%u)='tt) then block([tmin:%u[2],tmax:%u[3]],
     oklxf:1,
     map(lambda([%v],block([t:%v[1]],if ((tmin <= t) and (t<= tmax)) then lXf:append(lXf,[%v]))),lX),
   Ts:map(first,lXf))
   else lf:append(lf,[%u])),l),
 if oklxf=0 then lXf:lX,
 l:lf,
 N:makelist(i,i,1,length(Xs)+1),
 lsub:map(lambda([%u,%v],%u=%v),append(['t],Xs),N),
 lss:map(lambda([%u],args(%u)[1]=%w[args(%u)[2]]),lsub),
 lsub:append(lsub,['p=length(lXf[1])]),
 map(lambda([%u],block([],
  if (member(%u,Xs) or %u='p) then block([n:ev(%u,lsub)],
   leg:append(leg,[string(%u)]),
   lplot:append(lplot,[[discrete,Ts,map(lambda([%v],%v[n]),lXf)]]))
  else if atom(%u) then block([r:assoc(%u,Rel)],
    if not(is(r)=false) then block([rs:ev(r,lss),Xn],
     Xn:map(buildq([%rs:rs],lambda([%w],%rs)),lXf),
     leg:append(leg,[string(%u)]),
     lplot:append(lplot,[[discrete,Ts,Xn]])))
  else block([r:ev(%u,Rel,eval,lss),rs,Xn],
   Xn:map(buildq([%rs:r],lambda([%w],%rs)),lXf),
   leg:append(leg,[string(%u)]),
   lplot:append(lplot,[[discrete,Ts,Xn]]))
)),l),
 plot2d(lplot,leg),
 lplot)$
/* [wxMaxima: input   end   ] */

/* [wxMaxima: comment start ]
Looping de rouli
   [wxMaxima: comment end   ] */

/* [wxMaxima: input   start ] */
Vars0:ev(Vars,drx=1,dry=0,drz=0,dpx=0,dpy=1,dpz=0,dyx=0,dyy=0,dyz=1,omr=0,omp=0,omy=0,dx=0,dy=0,dz=0,x=0,y=0,z=0)$
COM:[efl=1.1*e,ebl=1.1*e,efr=e,ebr=e,eval,e=3.1]$
lX:rk(ev(SMeff,COM),Vars,Vars0,[t,0,1,0.01])$
XvsT(EDO,dpz,dz)$
/* [wxMaxima: input   end   ] */

/* [wxMaxima: comment start ]
Looping de tangage
   [wxMaxima: comment end   ] */

/* [wxMaxima: input   start ] */
Vars0:ev(Vars,drx=1,dry=0,drz=0,dpx=0,dpy=1,dpz=0,dyx=0,dyy=0,dyz=1,omr=0,omp=0,omy=0,dx=0,dy=0,dz=0,x=0,y=0,z=0)$
COM:[efl=1.1*e,ebl=e,efr=1.1*e,ebr=e,eval,e=3.1]$
lX:rk(ev(SMeff,COM),Vars,Vars0,[t,0,1,0.01])$
XvsT(EDO,drz,dz)$
/* [wxMaxima: input   end   ] */

/* [wxMaxima: comment start ]
Rotation propre
   [wxMaxima: comment end   ] */

/* [wxMaxima: input   start ] */
Vars0:ev(Vars,drx=1,dry=0,drz=0,dpx=0,dpy=1,dpz=0,dyx=0,dyy=0,dyz=1,omr=0,omp=0,omy=0,dx=0,dy=0,dz=0,x=0,y=0,z=0)$
COM:[efl=1.1*e,ebl=e,efr=e,ebr=1.1*e,eval,e=3.1]$
lX:rk(ev(SMeff,COM),Vars,Vars0,[t,0,20,0.01])$
XvsT(EDO,drx,dz)$
/* [wxMaxima: input   end   ] */

/* Maxima can't load/batch files which end with a comment! */
"Created with wxMaxima"$