function HW2_Matlab

% ENGN40 Homework #2 2020 Solutions



close all


%
%% Problem 3.1(students don't have to do this in MATLAB)
%

R = 1.5;
omega = 4*pi/15;
times = 5:0.1:20;
vx = -omega*R*sin(omega*(times-5));
vy = omega*R*cos(omega*(times-5));
ax = -omega^2*R*cos(omega*(times-5));
ay = -omega^2*R*sin(omega*(times-5));

plot(times,vx,'DisplayName','v_x=v_L','LineWidth',2)
hold on
plot(times,vy,'DisplayName','v_y=v_T','LineWidth',2)

ylabel('Velocity (m/s)');
xlabel('Time(s)');
axes1 = gca;
box(axes1,'on');
set(axes1,'FontSize',14,'XGrid','on','YGrid','on');
legend1 = legend(axes1,'show');
set(legend1,...
    'Position',[0.627875245250689 0.715370018975332 0.116959062619516 0.19607843075049],...
    'FontSize',14);
title('Fixed Orientation')

figure
vl = omega*R*ones(length(times),1);
vt = zeros(length(times),1);
plot(times,vl,'DisplayName','v_L','LineWidth',2);
hold on
plot(times,vt,'DisplayName','v_T','LineWidth',2);

ylabel('Velocity (m/s)');
xlabel('Time(s)');
axes1 = gca;
box(axes1,'on');
set(axes1,'FontSize',14,'XGrid','on','YGrid','on');
legend1 = legend(axes1,'show');
set(legend1,...
    'Position',[0.627875245250689 0.615370018975332 0.116959062619516 0.19607843075049],...
    'FontSize',14);
title('Circle with turn')


figure
plot(times,ax,'DisplayName','a_x=a_L','LineWidth',2)
hold on
plot(times,ay,'DisplayName','a_y=a_T','LineWidth',2)

ylabel('Acceleration (m/s^2)');
xlabel('Time(s)');
axes1 = gca;
box(axes1,'on');
set(axes1,'FontSize',14,'XGrid','on','YGrid','on');
legend1 = legend(axes1,'show');
set(legend1,...
    'Position',[0.627875245250689 0.715370018975332 0.116959062619516 0.19607843075049],...
    'FontSize',14);
title('Fixed Orientation')

at = omega^2*R*ones(length(times),1);
al = zeros(length(times),1);
figure
plot(times,al,'DisplayName','a_L','LineWidth',2);
hold on
plot(times,at,'DisplayName','a_T','LineWidth',2);

ylabel('Acceleration (m/s^2)');
xlabel('Time(s)');
axes1 = gca;
box(axes1,'on');
set(axes1,'FontSize',14,'XGrid','on','YGrid','on');
legend1 = legend(axes1,'show');
set(legend1,...
    'Position',[0.627875245250689 0.615370018975332 0.116959062619516 0.19607843075049],...
    'FontSize',14);
title('Circle with turn')

%% Problem 3.2

data1 = csvread('circle_fixedorientation.csv');
time = data1(:,1);
pos = data1(:,2:4);
vel = data1(:,5:7);
acc = data1(:,8:10);
yaw = data1(:,11);

figure
plot3(pos(:,1),pos(:,2),pos(:,3));
ylabel('y (m)');
xlabel('x (m)');
zlabel('z (m)');
axes1 = gca;
box(axes1,'on');
set(axes1,'FontSize',14,'XGrid','on','YGrid','on','ZGrid','on');
title('Fixed Orientation trajectory')


figure
plot1 = plot(time,vel(:,1:2));
ylabel('Velocity (m/s)');
xlabel('Time(s)');
axes1 = gca;
box(axes1,'on');
set(axes1,'FontSize',14,'XGrid','on','YGrid','on');
set(plot1(1),'DisplayName','v_L')
set(plot1(2),'DisplayName','v_T')
legend1 = legend(axes1,'show');
set(legend1,...
    'Position',[0.627875245250689 0.715370018975332 0.116959062619516 0.19607843075049],...
    'FontSize',14);
title('Fixed Orientation')

figure
plot1 = plot(time,acc(:,1:2));
ylabel('Acceleration (m/s^2)');
xlabel('Time(s)');
axes1 = gca;
box(axes1,'on');
set(axes1,'FontSize',14,'XGrid','on','YGrid','on');
set(plot1(1),'DisplayName','a_L')
set(plot1(2),'DisplayName','a_T')
legend1 = legend(axes1,'show');
set(legend1,...
    'Position',[0.627875245250689 0.715370018975332 0.116959062619516 0.19607843075049],...
    'FontSize',14);
title('Fixed Orientation')



data1 = csvread('circle_with_turn.csv');
time = data1(:,1);
pos = data1(:,2:4);
vel = data1(:,5:7);
acc = data1(:,8:10);
yaw = data1(:,11);

figure
plot3(pos(:,1),pos(:,2),pos(:,3));
ylabel('y (m)');
xlabel('x (m)');
zlabel('z (m)');
axes1 = gca;
box(axes1,'on');
set(axes1,'FontSize',14,'XGrid','on','YGrid','on','ZGrid','on');
title('Circle with turn trajectory')


figure
plot1 = plot(time,vel(:,1:2));
ylabel('Velocity (m/s)');
xlabel('Time(s)');
axes1 = gca;
box(axes1,'on');
set(axes1,'FontSize',14,'XGrid','on','YGrid','on');
set(plot1(1),'DisplayName','v_L')
set(plot1(2),'DisplayName','v_T')
legend1 = legend(axes1,'show');
set(legend1,...
    'Position',[0.627875245250689 0.715370018975332 0.116959062619516 0.19607843075049],...
    'FontSize',14);
title('Circle with turn')

figure
plot1 = plot(time,acc(:,1:2));
ylabel('Acceleration (m/s^2)');
xlabel('Time(s)');
axes1 = gca;
box(axes1,'on');
set(axes1,'FontSize',14,'XGrid','on','YGrid','on');
set(plot1(1),'DisplayName','a_L')
set(plot1(2),'DisplayName','a_T')
legend1 = legend(axes1,'show');
set(legend1,...
    'Position',[0.627875245250689 0.715370018975332 0.116959062619516 0.19607843075049],...
    'FontSize',14);
title('Circle with turn')



%% 3.3

data1 = csvread('circle_fixedorientation.csv');
time = data1(:,1);
pos = data1(:,2:4);
vel = data1(:,5:7);
acc = data1(:,8:10);
yaw = data1(:,11);

integrated_accels = cumtrapz(time,acc)

figure
plot1(1:2) = plot(time,vel(:,1:2));
hold on
plot1(3:4) = plot(time,integrated_accels(:,1:2));
ylabel('Velocity (m/s)');
xlabel('Time(s)');
axes1 = gca;
box(axes1,'on');
set(axes1,'FontSize',14,'XGrid','on','YGrid','on');
set(plot1(1),'DisplayName','v_L')
set(plot1(2),'DisplayName','v_T')
set(plot1(3),'DisplayName','Integrated a_L')
set(plot1(4),'DisplayName','Integrated a_T')
legend1 = legend(axes1,'show');
set(legend1,...
    'Position',[0.627875245250689 0.715370018975332 0.116959062619516 0.19607843075049],...
    'FontSize',14);
title('Fixed Orientation')


%% Problem 5

v0 = 0.26;
omega = 0.099;
Omega = 0.2*omega;
tstop = 223.5;

[times,sols] = ode45(@(t,w) bacterium(t,w,Omega,omega,v0),[0,tstop],[0,0]);

figure
plot(sols(:,1),sols(:,2),'DisplayName','Case (1)','LineWidth',2)
%
v0 = 0.26;
omega = 0.035;
Omega = 1.5*omega;
tstop = 400;

[times,sols] = ode45(@(t,w) bacterium(t,w,Omega,omega,v0),[0,tstop],[20,0]);
hold on
plot(sols(:,1),sols(:,2),'DisplayName','Case (2)','LineWidth',2)


v0 = 0.16;
omega = 0.028;
Omega = 2.39*omega;
tstop = 544.5;

[times,sols] = ode45(@(t,w) bacterium(t,w,Omega,omega,v0),[0,tstop],[35,30]);
plot(sols(:,1),sols(:,2),'DisplayName','Case (3)','LineWidth',2)

ylabel('y (\mu m)');
xlabel('x (\mu m)');
title('Predicted trajectories of listeria bacteria');
axes1 = gca;
box(axes1,'on');
set(axes1,'FontSize',14,'XGrid','on','YGrid','on');
legend1 = legend(axes1,'show');
set(legend1,'FontSize',16);


end
function dwdt = bacterium(t,w,Omega,omega,v0)

dxdt = -v0*sin(Omega/omega*sin(omega*t));
dydt =  v0*cos(Omega/omega*sin(omega*t));

dwdt = [dxdt;dydt];

end