NewtonianFrame N
RigidFrame A, B, C, D
Particle H
RigidBody E

Point AB(A), BC(B), CD(C), EN(E)

Constant mH, mE, IE, g
Constant h, r, LC, LB
Constant tF, qB0, qBF, kpA, kdA, kpB, kdB

Variable qA'', qB'', qC'', wx', wy', wz'
Variable WorkA', WorkB'

Specified TA, TB
Specified qAdesired', qBdesired'

H.SetMass(mH)

E.SetMass(mE)
E.SetInertia(Ecm, IE = 2/5*mE*r^2, IE, IE)

A.RotateY(N, qA)
B.RotateZ(A, qB)
% Pour C, attention de ne pas le définir à partir de N (la rotation qA ne serait alors pas considerée)
C.RotateZ(B, -qB - qC) 
% Pas besoin de la position angulaire pour E, juste de la vitesse et acc. ang. 
E.SetAngularVelocityAcceleration( A, wx*ax> + wy*ay> + wz*az> )

CD.Translate(No, h*ay> + LB*bx> + LC*cx>)
H.Translate(CD, 0>)
Ecm.Translate(CD, -r*ay>)
% Il y a plusieurs façon de s'assurer que EN est fixé sur E... 
%   - Définir EN comme EN(E)
%   - Utiliser la commande comme ci dessous
EN.Translate(Ecm, -r*ay>, E) 

% Équations de couple tel que défini dans le livre
SetDt(qAdesired = 2*pi*t/tF)
SetDt(qBdesired = qB0 + (qBF - qB0)*t/tF - (qBF - qB0)/(2*pi)*sin(2*pi*t/tF))
TA = -kpA*(qA - qAdesired) - kdA*(qA' - qAdesired')
TB = -kpB*(qB - qBdesired) - kdB*(qB' - qBdesired')

% Ajouter les forces/moments selon action-réaction
% Seulement les forces/moments important pour Kane sont utilisés. Attention de ne pas en oublier!! Faites vos DCLs!
A.AddTorque(N, TA*ay>)
B.AddTorque(A, TB*bz>)

% Contraintes
Hauteur = Dot(ny>, En.GetPosition(No))
Roulement[1] = Dot(ax>, En.GetVelocity(N))
Roulement[2] = Dot(az>, En.GetVelocity(N))

% On remplace qc', wx et wz par des expression de qA', qB' et wy pour faire Kane embedded
SolveDt([Dt(Hauteur); Roulement], qc', wx, wz)

% On fait Kane Embedded avec qA', qB' et wy (qc', wx et wz ne devrait plus être dans nos équations à ce stade-ci...)
SetGeneralizedSpeed(qA', qB', wy)
EOM = System.GetDynamicsKane()

% Vérification des équations selon la méthode manuelle. Assurez vous de pouvoir faire ça!!

% Kane selon qA' 
GeneralizedForcesqAp = Dot(TA*ay>, D(A.GetAngularVelocity(N),qA',N)) + Dot(TB*bz>, D(B.GetAngularVelocity(A),qA',N))
GeneralizedEffForcesqAp = Dot(mH*H.GetAcceleration(N), D(H.GetVelocity(N), qA', N)) + Dot(mE*Ecm.GetAcceleration(N), D(Ecm.GetVelocity(N), qA', N))+ Dot( Dot(E.GetInertia(Ecm), E.GetAngularAcceleration(N)) + cross(E.GetAngularVelocity(N) , Dot( E.GetInertia(Ecm), E.GetAngularVelocity(N))) , D( E.GetAngularVelocity(N), qA', N))
EOMmanuel[1] =  - GeneralizedForcesqAp + GeneralizedEffForcesqAp
% Comparaison par soustraction (la fonction explicit remplace tous les termes par leur plus simple expression... ça facilite la comparaison) 
explicit(EOM[1] - EOMmanuel[1])

% Kane selon qB'
GeneralizedForcesqBp = Dot(TA*ay>, D(A.GetAngularVelocity(N),qB',N)) + Dot(TB*bz>, D(B.GetAngularVelocity(A),qB',N))
GeneralizedEffForcesqBp = Dot(mH*H.GetAcceleration(N), D(H.GetVelocity(N), qB', N)) + Dot(mE*Ecm.GetAcceleration(N), D(Ecm.GetVelocity(N), qB', N))+ Dot( Dot(E.GetInertia(Ecm), E.GetAngularAcceleration(N)) + cross(E.GetAngularVelocity(N) , Dot( E.GetInertia(Ecm), E.GetAngularVelocity(N))) , D( E.GetAngularVelocity(N), qB', N))
EOMmanuel[2] =  - GeneralizedForcesqBp + GeneralizedEffForcesqBp
% Comparaison par soustraction (ici on utilise aussi la fonction expand pour calculer les expression entre parenthèse d'ordre 1 et 2... ça ne marche pas si on ne tiens pas compte de l'ordre 2)
expand(explicit(EOM[2] - EOMmanuel[2]),1:2)

% Constantes et valeurs initiales… 
Input r = 0.4 m, h = 0.6 m, LB = 0.8 m, LC = 1.2 m
Input mH = 800 kg, mE = 50 kg
Input tFinal = 25 sec, tStep = 0.1
Input qA = 0 deg, qB = 20 deg, qA' = 0 deg/sec, qB' = 0 deg/sec
SolveSetInputDt(Hauteur, qC = -5 deg)
Input wy = 0
input tF = 16 sec, qB0 = 20 deg, qBF = 60 deg
input kpA = 3000 N*m, kdA = 3 N*m/s, kpB = 3000 N*m, kdB  = 3 N*m/s
Input WorkA = 0 J, WorkB = 0 J

% Définition de valeurs utiles pour faire les graphiques
xE = Dot( nx> , Ecm.GetPosition(No))
zE = Dot( nz> , Ecm.GetPosition(No))

% Calcul du travail par intégration de la puissance et de l'énergie cinétique
WorkA' = Dot( TA*ay> , A.GetAngularVelocity(N) ) 
WorkB' = Dot( TB*bz> , B.GetAngularVelocity(A) ) 
Ke = 1/2*mH*Dot( H.GetVelocity(N) , H.GetVelocity(N) ) + 1/2*mE*Dot( Ecm.GetVelocity(N) , Ecm.GetVelocity(N) ) + 1/2*Dot( E.GetAngularVelocity(N), Dot( E.GetInertia(Ecm), E.GetAngularVelocity(N)))
explicit(Ke - system.getkineticenergy())
KeMinusWorkAB = Ke - WorkA - WorkB

Output xE m, zE m, KeMinusWorkAB J

% génération du code matlab
ODE(EOM, qA'', qB'', wy') HW21p11.m(kpA, kpB, kdA, kdB)

exit