%% Simple Forward-Facing Simulation Code for determining 0-60 mph times and max speed capability of a vehicle.
% Example solution code for Sim 1
% TA - Matt Klein, email: mpklein@ucdavis.edu
%% Assignment description
% This code simulates a simple ode that describes the 1d-longitudinal
% vehicle motion caused by the powertrain over a 0-60 mph full power run.  
% Currently, it uses a single final drive ratio as the transmission gear 
% ratio between the torque source (engine or e-motor) and the wheels. You 
% are tasked with updated the code such that it utilizes a 5-spd transmission
% prior to the final drive ratio.  The trick here will be to have the code
% update the gear ratio used in the v_dot equation based on the current
% engine speed.  Hence, if the engine speed is larger than some predefined
% limit (red-line) the transmission must shift up to the next gear.  Also,
% if you are already in the top gear then the speed must be held constant.
% Once you have successfully enabled the use of a 5-spd gear box you are
% to follow the requirements outlined in the provided assignment document.
% It is recommended that you make this file a function and then create a
% master script that call this function with provided inputs that are to be
% parametric study variables.  The output will be the variables of
% interest.
%
% You are to create your own code, but encouraged to develop it in a group
% effort, using the same lab groups you had for the engine tear down
% sections.
%% Start of the code
% This is how you set an M-file to being a function.  First "function" is
% written and this is then followed by the set of outputs of interest.  An
% equal sign is placed to the right and then the function name with a set
% of parentheses attached, whereby the set of inputs is placed into the
% parens.  Here only two outputs and three inputs are used.  The names of these
% variables must match the useage only within the function file, whereas
% when the function is called from a higher level script (or function) the
% position within the output or input array is all that matters.
function [zero_to_60_time,output_matrix] = test_code(FD,max_index,dt,v_end)

%% Define the constant system parameters
% The system parameters that are constant are defined within the function
% file.  Notice that FD has been commented as it is now being sent in as an
% input to the function.  If this was not commented out then whatever we
% sent in as the value of FD would get overwritten by the following FD.
m = 1200; % [kg], vehicle mass
g=9.8; % [m/s/s], gravitational acceleration constant
Cd=.3; % [], drag coefficient of vehicle body
Af=2; % [m^2], frontal area of the vehicle body
rho=1.2; % [kg/m^3], air density
%FD=3.55; % [], final drive gear ratio 
rw=.32; % [m], wheel radius

%% Torque source definition
% The torque scale may be used to play with changing the power level of the
% engine (e-motor).  The profile will be the same as given in the
% source_table, but the magnitude will change proportionately to
% torque_scale. Large torque_scale values should reduce 0-60 times.
torque_scale = 1.0; % [], use this to scale the torque source
% [rpm, output torque [N-m]]
source_table = [0:500:6000;torque_scale*[78,112,134,147,158,165,162,159,160,165,165,161,151]]'; 
max_eng_speed = max(source_table(:,1));

%% Gear box, [gear nums, gear ratios]
% GB is the set of gear ratios.  Column 1 contains the set of gear numbers,
% while Col2 contains the actual values of the gear ratios.
GB = [1:5;3.785,1.945,1.5,.994,.78]';
max_gear = max(GB(:,1));

%% Unit converters
% Some different unit converters were made for this particular assignment.
rad2rpm = 60/(2*pi); % convert rad/s to rev/min

%% Preallocate the vectors of interest 
% Not here that because we are using a while loop with a fixed time step if
% we change FD the total simulation time will change as the tiem vector
% will be the 0-60 time.  Hence, when sending t, v, eng_speed, or any other
% vector that is generated in the while loop back as an output to the
% master file the length of v for FD(1) will be different for FD(2) because
% the total time will be different.  This will cause problems in matlab if
% you are attempting to store the vectors into an array, but the vectors
% are varying in length.
eng_speed = NaN(max_index+1,1);
t = NaN(max_index+1,1);
v = NaN(max_index+1,1);
v_dot = NaN(max_index+1,1);
Fr = NaN(max_index+1,1);
Fd = NaN(max_index+1,1);
Ff = NaN(max_index+1,1);
tau_source = NaN(max_index+1,1);
%% Solve the ODE
% setup some inital variables
i=1; % index for time
j=1; % index for gear shifting, set at one initially to start in first gear.
v(i)=0.1;t(i)=0;
% v_end is set to whatever final speed you would like the simulation to
% stop at.  Note that there is an additional loop break that is based on a
% fixed amount of simulation time, in case the v_end is set to a value that
% is physically unattainable based on the chosen system parameters.  For
% example, if v_end is set at 200 m/s, for the given torque map, the vehicle
% will drag limit at best maybe 63 m/s (~140 mph).  Hence, the while loop
% would run forever if there was not also some other form of breaking the
% loop.
% Changes should be made to v_end and t_end in order to perform 0-60 or max
% speed studies.
t_end = (max_index-1)*dt; % [s], maximum allowed simulation time
% This loop solves for a(t) [v_dot] and then uses the simplest numerical
% solving scheme, called Forward Euler, to solver for v(t).  We perform the
% force balance by first solving for the value of all forces in the
% equation.  This includes the forward force (generated by the torque of the
% engine) and the losses, which include tire drag and aerodynamic drag.
zero_to_60_time = NaN;
while v(i)<v_end  && max(t)<t_end % run the simulation while the vehicle speed 
                                  % is less than the desired end speed and
                                  % the simulation time is less than the
                                  % maximum desired sim time.
    %% Determine the current engine speed.
    eng_speed(i)=(v(i)*GB(j,2)*FD/rw)*rad2rpm; % [RPM], eng_speed is calculated from the forward speed based on a kinematic constraint.
    %% Performing shifting if needed.
    if eng_speed(i)>max_eng_speed && j<max_gear
        j=j+1;
        eng_speed(i)=(v(i)*GB(j,2)*FD/rw)*rad2rpm; % [RPM], eng_speed needs to be re-calculated if a shift occurs.
    end
        
    %% Determine the available torque given the current engine speed
    % interpolate to find the currently available torque based upon the
    % speed
    tau_source(i) = interp1(source_table(:,1),source_table(:,2),eng_speed(i),'linear','extrap');
    
    %% Compute the forces
    % solve for the drag and rolling resistances, note there is no grade
    % loss, hence assuming flat ground
    Fd(i) = .5*rho*Af*Cd*(v(i)^2);
    Fr(i) = m*g*(.01*(1+v(i)*3.6/160));
    % compute the avaiable forward force
    Ff(i) = tau_source(i)*FD/rw;
    
    %% Perform the force balance to solve for the generated acceleration
    % ode - longitudinal acceleration of the vehicle
    v_dot(i) = 1/m*(Ff(i) - Fd(i) - Fr(i)); % a(t) is calculated using Newton's law
    
    %% If j=5 and the eng_speed is max'd out we need to hold the speed constant
    % It is incorrect to say that eng_speed is constant as this will not
    % stop the vehicle acceleration.  v_dot must be set to zero to stop
    % acceleration.
    % This is basically a rev-limiter, which real cars have.
    if j==5 && eng_speed(i)>max_eng_speed
        v_dot(i)=0;
    end
    %% Use Forward-Euler
    % Note: all this is saying is that if dt is small enough we may
    % approximate the acceleration as constant over that time interval and
    % then say that the new velocity is equal to my old velocity plus the
    % change in velocity over the previous time increment.  This is very
    % intuitive, but it must be noted that it is also a compuationally
    % inefficient numerical method.  But, for this assignment it works
    % fine.
    % update the new velocity
    v(i+1) = v(i) + v_dot(i)*dt;
    % update the new time
    t(i+1) = t(i) + dt;
        
    %% Increment the loop counter   
    i=i+1;
    
end
%% Get zero-to-sixty time
if max(v) > 26
    [minval,ind_t] = min(abs(v-26.82));
    zero_to_60_time = t(ind_t);
end
% compute last values to match length of all vectors
eng_speed(i)=(v(i)*GB(j,2)*FD/rw)*rad2rpm;
tau_source(i) = interp1(source_table(:,1),source_table(:,2),eng_speed(i));
Fd(i) = .5*rho*Af*Cd*(v(i)^2);
Fr(i) = m*g*(.01*(1+v(i)*3.6/160));
Ff(i) = tau_source(i)*FD/rw;

output_matrix = [t,v,eng_speed,Fd,Fr,Ff];
test = output_matrix;