%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 4 Corners Model With Suspension Simulation
% Plots - by Dr. Efstathios Velenis & Bernat Carreras
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%re%%%%%%%%

figure2 = figure;
set(figure2,'DoubleBuffer','on','position',[410 40 800 600]);

subplot(3,5,1)
axis([0,max(t),0,50])
plot(t,V*3.6) 
xlabel('sec')
ylabel('km/h')
title('Speed')

subplot(3,5,11)
axis([0,max(t),30,50])
plot(t,(z-h)*1000)
xlabel('sec')
ylabel('mm')
title('Z-Motion cdg')

subplot(3,5,6)
plot(t,(deltaFL+deltaFR)/2*180/pi)
axis([0,max(t),-30,30])
xlabel('sec')
ylabel('deg')
title('Steering input')

subplot(3,5,2)
plot(t,theta*180/pi)
axis([0,max(t),-4,4])
xlabel('sec')
ylabel('deg')
title('Pitch')

subplot(3,5,7)
title('Roll')
plot(t,phi*180/pi)
axis([0,max(t),-20,20])
xlabel('sec')
ylabel('deg')
title('Roll')

subplot(3,5,12)
plot(t, [0;diff(ydot)])
%axis([0,max(t),-0.02,0.02])
xlabel('sec')
ylabel('-')
title('Lateral acceleration')

subplot(3,5,3)
axis([min(t),max(t),0,150])
plot(t,omegaFL,'b') 
hold on
plot(t,omegaFR,'r')
xlabel('sec')
ylabel('rad/s')
title('Wheel speed F')
grid on

subplot(3,5,4)
axis([min(t),max(t),0,150])
plot(t,omegaRL,'b')
hold on
plot(t,omegaRR,'r')
xlabel('sec')
ylabel('rad/s')
title('Wheel speed R')
grid on

subplot(3,5,13)
plot(t,fFLz/9.81,'b')
hold on
plot(t,fFRz/9.81,'r')
axis([min(t),max(t),0,600])
xlabel('sec')
ylabel('Kg')
title('Load F')
grid on

subplot(3,5,14)
plot(t,fRLz/9.81,'b')
hold on
plot(t,fRRz/9.81,'r')
axis([min(t),max(t),0,600])
xlabel('sec')
ylabel('Kg')
title('Load R')
grid on

subplot(3,5,8)
title('Slip F')
plot(t,sFL,'b')
hold on
plot(t,sFR,'r')
axis([min(t),max(t),0,0.4])
xlabel('sec')
ylabel('%')
title('Slip F')
grid on

subplot(3,5,9)
title('Slip F')
plot(t,sRL,'b')
hold on
plot(t,sRR,'r')
axis([min(t),max(t),0,0.4])
xlabel('sec')
ylabel('%')
title('Slip R')
grid on

subplot(3,5,5)
plot(t,psidot,'b')
axis([min(t),max(t),-2,2])
xlabel('sec')
ylabel('rad/s')
title('Psi dot')
grid on

subplot(3,5,15)
axis([min(t),max(t),-20,20])
plot(t,((z-h)+lf*sin(theta))*1000,'b')
hold on
plot(t,((z-h)-lr*sin(theta))*1000,'r')
xlabel('sec')
ylabel('mm')
title('Z-Motion')
grid on

subplot(3,5,10)
plot(t,beta*180/pi,'b')
axis([min(t),max(t),-10,10])
xlabel('sec')
ylabel('deg')
title('Side Slip')
grid on