% Code to plot simulation results from sdl_vehicle_4wd_testbed
%% Plot Description:
%
% The plot below shows the results of a four-wheel drive vehicle
% accelerating from rest with all four wheels on a low friction surface,
% such as ice. Two meters in front of the vehicle starting position is a
% high friction surface, such as dry tarmac. Open differentials are used on
% the front and rear and a viscous coupling is used on the center
% differential. As a result, all four wheels slip until all reach a high
% friction surface.

% Copyright 2016 The MathWorks, Inc.

% Generate simulation results if they don't exist
if ~exist('simlog_sdl_vehicle_4wd_testbed', 'var')
    sim('sdl_vehicle_4wd_testbed')
end

% Reuse figure if it exists, else create new figure
if ~exist('h1_sdl_vehicle_4wd_testbed', 'var') || ...
        ~isgraphics(h1_sdl_vehicle_4wd_testbed, 'figure')
    h1_sdl_vehicle_4wd_testbed = figure('Name', 'sdl_vehicle_4wd_testbed');
end
figure(h1_sdl_vehicle_4wd_testbed)
clf(h1_sdl_vehicle_4wd_testbed)

% Get simulation results
simlog_t = simlog_sdl_vehicle_4wd_testbed.Front_Tires.Tire_Left.A.w.series.time;
simlog_wLF = simlog_sdl_vehicle_4wd_testbed.Front_Tires.Tire_Left.A.w.series.values('rad/s');
simlog_wRF = simlog_sdl_vehicle_4wd_testbed.Front_Tires.Tire_Right.A.w.series.values('rad/s');
simlog_wLR = simlog_sdl_vehicle_4wd_testbed.Rear_Tires.Tire_Left.A.w.series.values('rad/s');
simlog_wRR = simlog_sdl_vehicle_4wd_testbed.Rear_Tires.Tire_Right.A.w.series.values('rad/s');
simlog_vVeh = simlog_sdl_vehicle_4wd_testbed.Vehicle_Body.Vehicle_Body.V.series.values('m/s');
temp_tirebh = find_system(simlog_sdl_vehicle_4wd_testbed.id,'regexp','on','Name','Tire Left');
temp_LFrad = str2double(char(get_param(temp_tirebh(1),'r_e')));

temp_whlSurf = logsout_sdl_vehicle_4wd_testbed.get('Whl_Surface');

% Plot results
simlog_handles(1) = subplot(2, 1, 1);
plot(simlog_t, simlog_vVeh,'k','LineWidth', 3)
hold on
plot(simlog_t, simlog_wLF*temp_LFrad,'LineWidth', 2)
plot(simlog_t, simlog_wRF*temp_LFrad,'--','LineWidth', 2)
plot(simlog_t, simlog_wLR*temp_LFrad,'-','LineWidth', 1)
plot(simlog_t, simlog_wRR*temp_LFrad,'--','LineWidth', 1)
hold off
grid on
title('Vehicle and Tangential Wheel Speeds')
ylabel('Speed (m/s)')
legend({'Vehicle','LF','RF','LR','RR'},'Location','Best');


simlog_handles(2) = subplot(2, 1, 2);
lin_h=plot(temp_whlSurf.Values.Time, temp_whlSurf.Values.Data, 'LineWidth',2);
set(lin_h(2),'LineStyle','--','LineWidth',2);
set(lin_h(3),'LineStyle','-','LineWidth',1);
set(lin_h(4),'LineStyle','--','LineWidth',1);
grid on
title('Surface')
ylabel('')
xlabel('Time (s)')
set(gca,'YLim',[0.9 4.1],'YTick',1:4)
set(gca,'YTickLabel',{'Ice' 'Snow' 'Wet' 'Dry'})
linkaxes(simlog_handles, 'x')
legend({'LF','RF','LR','RR'},'Location','Best');


% Remove temporary variables
clear simlog_t simlog_handles
clear simlog_wLF simlog_wRF simlog_wLR simlog_wRR temp_whlSurf temp_LFrad
clear simlog_vVeh