% M-file name: wheelset.m 
% M-file type: Function file 
% This function file identifies the single-point (tread contact) and the two-point (tread and  flange contact) 
%equations that constitute the mathematical model for the dynamics of a single 
% wheelset rolling on a straight track. The equations are written in state space form. 
% This function is called by the function file 'equations.m' for solving the differential equations
% constituting the motion of wheelset. The wheelset lateral and yaw displacements and 
% velocities, and the left and right rail lateral displacements are provided as inputs to this 
% function in order to solve for the wheelset. Additionally, the wheelset suspension forces and
% moments are provided as inputs.

% This function calls the following functions 
% 'rolling_radius', 'contact_angle', 'onept_creep', 'onept_normal', 'twopt_creep', and
% 'twopt_normal'.

function [xdot1,xdot2,xdot3,xdot4,xdot5,xdot6,phi]=wheelset(x1,x2,x3,x4,x5,x6,Fsuspyw,Msuspzw)

% Parameters used for simulation

% V: Forward velocity of wheelset (m/sec)
% lambda: wheel conicity
% a: wheel base (m)
% r0: Centered rolling radius of the wheel (m)
% yfc: Flange clearance or flange width (m)
% yfctol: Lateral tolerance added to yfc in order to facilitate numerical simulation (m)
% mw: Mass of wheelset (kg)
% Iwz: Yaw principal mass moment of inertia of wheelset (kg-m2)
% Iwy: Pitch principal mass moment of inertia of wheelset (kg-m2)
% krail: Effective lateral stiffness of rail (N/m)
% crail: Effective lateral damping of rail (N/m)
% g: Acceleration due to gravity (m/s2)

% Indicating the global nature of the variables. This means that the value of the variables need
% not be specified in this function file. This value is automatically obtained from the main file
% 'single_wheelset.m'.

global V lambda a r0 yfc yfctol mw Iwz Iwy krail crail g;

% Nomenclature:

% rlt: Rolling radius at left wheel tread contact patch 
% rlf: Rolling radius at left wheel flange contact patch 
% rrt: Rolling radius at right wheel tread contact patch 
% rrf: Rolling radius at right wheel flange contact patch 
% deltalt: Contact angle at left wheel tread contact patch
% deltalf: Contact angle at left wheel flange contact patch 
% deltart: Contact angle at right wheel tread contact patch 
% deltarf: Contact angle at right wheel flange contact patch 
% rl: Rolling radius at left wheel contact patch (for single-point contact) 
% rr: Rolling radius at right wheel contact patch (for single-point contact) 
% deltal: Effective contact angle at left wheel contact patch after compensating for wheelset roll
% angle (for single-point contact) 
% deltatl: Effective contact angle at left wheel tread contact patch after compensating for
% wheelset roll angle (for two-point contact)
% deltafl: Effective contact angle at left wheel flange contact patch after compensating for
% wheelset roll angle (for two-point contact)
% deltar: Effective contact angle at right wheel contact patch after compensating for wheelset
% roll angle (for single-point contact)
% deltatr: Effective contact angle at right wheel tread contact patch after compensating for
% wheelset roll angle (for two-point contact)
% deltafr: Effective contact angle at right wheel flange contact patch after compensating for
% wheelset roll angle (for two-point contact)
% lambdal: Slope of tangent at left wheel contact patch (for single-point contact)
% lambdalt: Slope of tangent at left wheel tread contact patch (for two-point contact)
% lambdalf: Slope of tangent at left wheel flange contact patch (for two-point contact)
% lambdar: Slope of tangent at right wheel contact patch (for single-point contact)
% lambdart: Slope of tangent at right wheel tread contact patch (for two-point contact)
% lambdarf: Slope of tangent at right wheel flange contact patch (for two-point contact)
% phi: Wheelset roll angle
% dphi: Rate of change of wheelset roll angle
% etaxl: Longitudinal creepage at left wheel contact patch (for single-point contact)
% etaxlt: Longitudinal creepage at left wheel tread contact patch (for two-point contact)
% etaxlf: Longitudinal creepage at left wheel flange contact patch (for two-point contact)
% etayl: Lateral creepage at left wheel contact patch (for single-point contact)
% etaylt: Lateral creepage at left wheel tread contact patch (for two-point contact)
% etaylf: Lateral creepage at left wheel flange contact patch (for two-point contact)
% etaspl: Spin creepage at left wheel contact patch (for single-point contact)
% etasplt: Spin creepage at left wheel tread contact patch (for two-point contact)
% etasplf: Spin creepage at left wheel flange contact patch (for two-point contact)
% etaxr: Longitudinal creepage at right wheel contact patch (for single-point contact)
% etaxrt: Longitudinal creepage at right wheel tread contact patch (for two-point contact)
% etaxrf: Longitudinal creepage at right wheel flange contact patch (for two-point contact)
% etayr: Lateral creepage at right wheel contact patch (for single-point contact)
% etayrt: Lateral creepage at right wheel tread contact patch (for two-point contact)
% etayrf: Lateral creepage at right wheel flange contact patch (for two-point contact)
% etaspr: Spin creepage at right wheel contact patch (for single-point contact)
% etasprt: Spin creepage at right wheel tread contact patch (for two-point contact)
% etasprf: Spin creepage at right wheel flange contact patch (for two-point contact)
% Fcxl: Longitudinal creep force on left wheel contact patch (for single-point contact)
% Fcxlt: Longitudinal creep force on left wheel tread contact patch (for two-point contact)
% Fcxlf: Longitudinal creep force on left wheel flange contact patch (for two-point contact)
% Fcyl: Lateral creep force on left wheel contact patch (for single-point contact)
% Fcylt: Lateral creep force on left wheel tread contact patch (for two-point contact)
% Fcylf: Lateral creep force on left wheel flange contact patch (for two-point contact)
% Fczl: Vertical creep force on left wheel contact patch (for single-point contact)
% Fczlt: Vertical creep force on left wheel tread contact patch (for two-point contact)
% Fczlf: Vertical creep force on left wheel flange contact patch (for two-point contact)
% Mcxl: Longitudinal creep moment on left wheel contact patch (for single-point contact)

% Mcxlt: Longitudinal creep moment on left wheel tread contact patch (for two-point contact)
% Mcxlf: Longitudinal creep moment on left wheel flange contact patch (for two-point contact)
% Mcyl: Lateral creep moment on left wheel contact patch (for single-point contact)
% Mcylt: Lateral creep moment on left wheel tread contact patch (for two-point contact)
% Mcylf: Lateral creep moment on left wheel flange contact patch (for two-point contact)
% Mczl: Vertical creep moment on left wheel contact patch (for single-point contact)
% Mczlt: Vertical creep moment on left wheel tread contact patch (for two-point contact)
% Mczlf: Vertical creep moment on left wheel flange contact patch (for two-point contact)
% Fcxr: Longitudinal creep force on right wheel contact patch (for single-point contact)
% Fcxrt: Longitudinal creep force on right wheel tread contact patch (for two-point contact)
% Fcxrf: Longitudinal creep force on right wheel flange contact patch (for two-point contact)
% Fcyr: Lateral creep force on right wheel contact patch (for single-point contact)
% Fcyrt: Lateral creep force on right wheel tread contact patch (for two-point contact)
% Fcyrf: Lateral creep force on right wheel flange contact patch (for two-point contact)
% Fczr: Vertical creep force on right wheel contact patch (for single-point contact)
% Fczrt: Vertical creep force on right wheel tread contact patch (for two-point contact)
% Fczrf: Vertical creep force on right wheel flange contact patch (for two-point contact)
% Mcxr: Longitudinal creep moment on right wheel contact patch (for single-point contact)
% Mcxrt: Longitudinal creep moment on right wheel tread contact patch (for two-point contact)
% Mcxrf: Longitudinal creep moment on right wheel flange contact patch (for two-point contact)
% Mcyr: Lateral creep moment on right wheel contact patch (for single-point contact)
% Mcyrt: Lateral creep moment on right wheel tread contact patch (for two-point contact)
% Mcyrf: Lateral creep moment on right wheel flange contact patch (for two-point contact)
% Mczr: Vertical creep moment on right wheel contact patch (for single-point contact)
% Mczrt: Vertical creep moment on right wheel tread contact patch (for two-point contact)
% Mczrf: Vertical creep moment on right wheel flange contact patch (for two-point contact)
% Fnyl: Lateral normal force on left wheel contact patch (for single-point contact)
% Fnylt: Lateral normal force on left wheel tread contact patch (for two-point contact)
% Fnylf: Lateral normal force on left wheel flange contact patch (for two-point contact)
% Fnzl: Vertical normal force on left wheel contact patch (for single-point contact)
% Fnzlt: Vertical normal force on left wheel tread contact patch (for two-point contact)
% Fnzlf: Vertical normal force on left wheel flange contact patch (for two-point contact)
% Fnyr: Lateral normal force on right wheel contact patch (for single-point contact)
% Fnyrt: Lateral normal force on right wheel tread contact patch (for two-point contact)
% Fnyrf: Lateral normal force on right wheel flange contact patch (for two-point contact)
% Fnzr: Vertical normal force on right wheel contact patch (for single-point contact)
% Fnzrt: Vertical normal force on right wheel tread contact patch (for two-point contact)
% Fnzrf: Vertical normal force on right wheel flange contact patch (for two-point contact)

% Obtaining rolling radii and contact angles at left and right wheel contact patches from
% functions 'rolling_radius' and 'contact_angle'

[rlt,rlf,rrt,rrf]=rolling_radius(x1,x1-x5,x6-x1);
[deltalt,deltalf,deltart,deltarf]=contact_angle(x1,x1-x5,x6-x1);

% Choose set of differential equations to be solved depending on single-point or two-point 
% contact condition at end of previous time step 
% Single-point contact equations -Right wheel flange contact and Left wheel tread contact 
if (x6-x1)>(yfc+yfctol) 
% Assigning variable names to rolling radii at left and right wheel contact patches
rl=rlt;
rr=rrf;
% Computing tangent slopes at left and right wheel contact patches 
lambdal=tan(deltalt);
lambdar=tan(deltarf);
% Computing wheelset roll angle and rate of change of wheelset roll angle
phi=(rl-rr)/(2*a);
dphi=(lambdal+lambdar)*(x3/(2*a)); 
% Computing effective contact angles at left and right wheel contact patches by
% compensating for wheelset roll angle
deltal=deltalt+phi;
deltar=deltarf-phi;
% Computing longitudinal, lateral, and spin creepages at left wheel tread contact patch
etaxl=-a*(x4/V)+(1-rl/r0);
etayl=((x3/V)-x2*(rl/r0)+rl*dphi/V)/cos(deltal);
etaspl=((x4/V)+(phi/r0))*cos(deltal)-(1/r0)*sin(deltal);

% Computing longitudinal, lateral, and spin creepages at right wheel flange contact patch
etaxr=a*(x4/V)+(1-rr/r0);
etayr=((x3/V)-x2*(rr/r0)+rr*dphi/V)/cos(deltar);
etaspr= ((x4/V)+(phi/r0))*cos(deltar)+(1/r0)*sin(deltar);

% Obtaining creep forces and moments at left and right wheel contact patches from function
% 'onept_creep'
[Fcxl,Fcyl,Fczl,Mcxl,Mcyl,Mczl,Fcxr,Fcyr,Fczr,Mcxr,Mcyr,Mczr]=onept_creep(etaxl,etayl,etaspl,etaxr,etayr,etaspr,deltal,deltar);
% Obtaining normal forces at left and right wheel contact patches from function 
% 'onept_normal' 
[Fnyl,Fnzl,Fnyr,Fnzr]=onept_normal(x2,x4,deltal,deltar,rl,rr,Fcxl,Fcxr,Fcyl,Fcyr,Fczl,Fczr,Mcyl,Mcyr); 
% The single-point equations in state space form 
xdot1=x3; 
xdot2=x4; 
xdot3=1/mw*(Fcyl+Fcyr+Fnyl+Fnyr+Fsuspyw-mw*g*lambda*x1/a); 
xdot4=1/Iwz*((-Iwy*V/r0*dphi)-a*(Fcxl-Fcxr)-x2*((a-rl*tan(deltal))*(Fcyl+Fnyl)-(a-rr*tan(deltar))*(Fcyr+Fnyr))+Mczl+Mczr+Msuspzw);
xdot5=(1/crail)*(-Fnyl-Fcyl-krail*x5);
xdot6=(1/crail)*(-Fnyr-Fcyr-krail*x6);


% Two-point contact equations -Right wheel two-point contact and Left wheel tread % contact 
elseif (x6-x1)>yfc 
% Assigning variable name to rolling radius at left wheel contact patch 
rl=rlt; 
% Computing tangent slopes at left and right wheel contact patches 
lambdal=tan(deltalt); 
lambdart=tan(deltart); 
lambdarf=tan(deltarf); 
% Computing wheelset roll angle and rate of change of wheelset roll angle 
phi=(rl-rrt)/(2*a); 
dphi=(lambdal+lambdarf)* (x3/(2*a)); 
% Computing effective contact angles at left and right wheel contact patches by 
% compensating for wheelset roll angle 
deltal=deltalt+phi;