%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% ME3308 - ME5541
% PRACTICAL SESSION 4
% Single-Track Model Simulation
% EOM - by Dr. Efstathios Velenis
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


function de = FWeom(t,e)

global m lf lr Jp Jx Jy tf tr W Jwf Jwr Rwf Rwr

global deltaFL deltaFR deltaRL deltaRR

x = e(1);
xdot = e(2);
y = e(3);
ydot = e(4);
z = e(5);
zdot = e(6);
psi = e(7);
psidot = e(8);
theta = e(9);
thetadot = e(10);
phi = e(11);
phidot = e(12);
omegaFL = e(13);
omegaFR = e(14);
omegaRL = e(15);
omegaRR = e(16);

V = sqrt(xdot^2 + ydot^2);
rho = atan2(ydot,xdot);
beta = rho-psi;

%============================================================
%   Input
% delta, Moment Pneumàtic
%============================================================

delta = steer_fun(t);
    deltaFL = delta(1);
    deltaFR = delta(2);
    deltaRL = delta(3);
    deltaRR = delta(4);
torque = accbrk_fun(t,omegaFL,omegaFR, omegaRL, omegaRR);
    TFL = torque(1);
    TFR = torque(2);
    TRL = torque(3);
    TRR = torque(4);
    

    

%============================================================
%   Vij
% Càlcul velocitats
%============================================================

rF = sqrt(lf^2 + (tf/2)^2);
angF = atan((tf/2)/lf);
rR = sqrt(lr^2 + (tr/2)^2);
angR = atan((tr/2)/lr);

VFLx = V*cos(beta - deltaFL) + psidot*rF*cos(pi/2 - deltaFL + angF);
VFLy = V*sin(beta - deltaFL) + psidot*rF*sin(pi/2 - deltaFL + angF);

VFRx = V*cos(beta - deltaFR) + psidot*rF*cos(pi/2 - deltaFR - angF);
VFRy = V*sin(beta - deltaFR) + psidot*rF*sin(pi/2 - deltaFR - angF);

VRLx = V*cos(beta - deltaRL) - psidot*rR*cos(pi/2 - deltaRL - angR);
VRLy = V*sin(beta - deltaRL) - psidot*rR*sin(pi/2 - deltaRL - angR);

VRRx = V*cos(beta - deltaRR) - psidot*rR*cos(pi/2 - deltaRL + angR);
VRRy = V*sin(beta - deltaRR) - psidot*rR*sin(pi/2 - deltaRL + angR);


%============================================================
%   Slip
% Angle de patinatge
%============================================================
slip = slip_fun(VFLx, VFLy, VFRx, VFRy, VRLx,VRLy, VRRx, VRRy, omegaFL, omegaFR, omegaRL, omegaRR);

sFLx = slip(1);
sFLy = slip(2);
sFL  = slip(3);

sFRx = slip(4);
sFRy = slip(5);
sFR  = slip(6);

sRLx = slip(7);
sRLy = slip(8);
sRL  = slip(9);

sRRx = slip(10);
sRRy = slip(11);
sRR  = slip(12);


%============================================================
%   Friction
% Força de fricció
%============================================================


friction = friction_fun(sFLx, sFLy, sFL, sFRx, sFRy, sFR, sRLx, sRLy, sRL, sRRx, sRRy, sRR, z, zdot, theta, thetadot, phi, phidot);

fFLx = friction(1);
fFLy = friction(2);
fFL  = friction(3);

fFRx = friction(4);
fFRy = friction(5);
fFR  = friction(6);

fRLx = friction(7);
fRLy = friction(8);
fRL  = friction(9);

fRRx = friction(10);
fRRy = friction(11);
fRR  = friction(12);

fFLz = friction(13);
fFRz = friction(14);
fRLz = friction(15);
fRRz = friction(16);

%============================================================
%   EOM
% Equacions de moviment
%============================================================
t;

de1 = xdot;
de2 = (fFLx*cos(deltaFL+psi) - fFLy*sin(deltaFL+psi) + fFRx*cos(deltaFR+psi)...
    - fFRy*sin(deltaFR+psi) + fRLx*cos(psi+deltaRL) - fRLy*sin(psi+deltaRL)...
    + fRRx*cos(deltaRR+psi) - fRRy*sin(deltaRR+psi))/m;
de3 = ydot;
de4 = (fFLx*sin(deltaFL+psi) + fFLy*cos(deltaFL+psi) + fFRx*sin(deltaFR+psi)...
    + fFRy*cos(deltaFR+psi) + fRLx*sin(psi+deltaRL) + fRLy*cos(psi+deltaRL)...
    + fRRx*sin(psi+deltaRR) + fRRy*cos(psi+deltaRR))/m;
de5 = zdot;
de6 = (fFLz + fFRz + fRLz + fRRz - W)/m;
de7 = psidot;
de8 = ((-fFLx*cos(deltaFL) + fFLy*sin(deltaFL) + fFRx*cos(deltaFR) - fFRy*sin(deltaFR))*(tf/2)...
      +(fFLx*sin(deltaFL) + fFLy*cos(deltaFL) + fFRx*sin(deltaFR) + fFRy*cos(deltaFR))*(lf)...
    +(-fRLx*cos(deltaRL) + fRLy*sin(deltaRL) + fRRx*cos(deltaRR) - fRRy*sin(deltaRR))*(tr/2)...
    +(-fRLx*sin(deltaRL) - fRLy*cos(deltaRL) - fRRx*sin(deltaRR) - fRRy*cos(deltaRR))*(lr))/Jp;
de9 = thetadot;
de10 = ((fFLx*cos(deltaFL) - fFLy*sin(deltaFL)+fFRx*cos(deltaFR) - fFRy*sin(deltaFR))*z ...
    + (fFLz+fFRz)*lf + (fRLx*cos(deltaRL)+fRLy*sin(deltaRL)+fRRx*cos(deltaRR)-fRRy*sin(deltaRR))*z - (fRLz+fRRz)*lr)/Jy;
de11 = phidot;
de12 = ((fFLx*sin(deltaFL) + fFLy*cos(deltaFL)+ fFRx*sin(deltaFR)+ fFRy*cos(deltaFR))*z ...
    + (fFLz-fFRz)*(tf/2) + (-fRLx*sin(deltaRL) + fRLy*cos(deltaRL)+ fRRx*sin(deltaRR)+ fRRy*cos(deltaRR))*z + (fRLz-fRRz)*(tr/2))/Jx;
de13 = (TFL - fFLx*Rwf)/Jwf;
de14 = (TFR - fFRx*Rwf)/Jwf;
de15 = (TRL - fRLx*Rwr)/Jwr;
de16 = (TRR - fRRx*Rwr)/Jwr;

de = [de1 de2 de3 de4 de5 de6 de7 de8 de9 de10 de11 de12 de13 de14 de15 de16]';


