function HW3
% ENGN0040 Dynamics and Vibrations
% Solutions to HW set #3, 2020


close all

%% Problem 4
theta0 = (90+25)*pi/180;  % Initial angle (radians)
L0 = 6/200;               % Constant specifying animal's length
dL = 0.75/100;            % Constant specifying change in animals length
g = 9.81;                 % Gravitational accel
beta = 25*pi/180;         % Constant in length calculation
omega0 = 15.95;            % Initial angular speed (rad/s)

% Solve the ODE
initial_w = [theta0,-omega0];
[times,sols] = ode45(@(t,w) cockroach(t,w,L0,dL,g,beta), [0,0.13],initial_w);

% Post processing
 r = L0 - dL*sin(sols(:,1)-beta);
 x = r.*sin(sols(:,1));
 y = -r.*cos(sols(:,1));
 
 figure
 plot(x,y,'DisplayName','Variable length','LineWidth',2);

 % Repeat the calculation with constant length
 theta0 = (90+25)*pi/180;
L0 = 6/200;
dL = 0;
g = 9.81;
beta = 25*pi/180;
omega0 = 15.95;

initial_w = [theta0,-omega0];
[times,sols] = ode45(@(t,w) cockroach(t,w,L0,dL,g,beta), [0,0.13],initial_w);

  r = L0 - dL*sin(sols(:,1)-beta);
 x = r.*sin(sols(:,1));
 y = -r.*cos(sols(:,1));
 hold on
  plot(x,y,'DisplayName','Constant length','LineWidth',2);
 
  axes1 = gca;
ylabel('y');
xlabel('x');
box(axes1,'on');
axis equal
set(axes1,'FontSize',14,'XGrid','on','YGrid','on');
legend1 = legend(axes1,'show');
set(legend1,...
    'Position',[0.253512131145383 0.774831649515566 0.323754782535107 0.11742423926339]);
  
  
%% Problem 5  
R = 0.5;
g = 9.81;
m = 0.25;
mu = 0.75;
Omega = 2*sqrt(g/R);
options = odeset('RelTol',0.000001,'Event',@(t,w) dropoff(t,w,R,g))
[times,sols] = ode45(@(t,w) mixer(t,w,mu,g,R,Omega),[0:0.01:5],[0,0],options);

figure
plot(times*sqrt(g/R),sols(:,1),'LineWidth',2);
axes1 = gca;
ylabel('Angle $\theta$','Interpreter','latex');
xlabel('Time $t\sqrt{(g/R)}$','Interpreter','latex');
box(axes1,'on');
set(axes1,'FontSize',14,'XGrid','on','YGrid','on');
figure
plot(times*sqrt(g/R),sols(:,2)*sqrt(R/g),'LineWidth',2);
axes1 = gca;
ylabel('Angular speed $\omega\sqrt{g/R}$','Interpreter','latex');
xlabel('Time $t\sqrt{(g/R)}$','Interpreter','latex');
box(axes1,'on');
set(axes1,'FontSize',14,'XGrid','on','YGrid','on');
 N = m*R*sols(:,2).^2 + m*g*cos(sols(:,1));
figure
plot(times*sqrt(g/R),N/(m*g),'LineWidth',2);
axes1 = gca;
ylabel('Contact force N/(m g)','Interpreter','latex');
xlabel('Time $t\sqrt{(g/R)}$','Interpreter','latex');
box(axes1,'on');
set(axes1,'FontSize',14,'XGrid','on','YGrid','on');


end
function dwdt = cockroach(t,w,L0,dL,g,beta)
  q = w(1); omega = w(2);
  
  r = L0 - dL*sin(q-beta);
  rdot = -dL*omega*cos(q-beta);
  
  dqdt = omega;
  domegadt = -(g*sin(q) + 2*rdot*omega)/r;

  dwdt = [dqdt;domegadt];


end
function dwdt = mixer(t,w,mu,g,R,Omega)
 theta = w(1);  omega = w(2);
 
 dthetadt = omega;

 domegadt = (mu*(R*omega^2+g*cos(theta)) - g*sin(theta))/R;
 
 if (omega>Omega)
     domegadt = 0;
 end
 
 dwdt = [dthetadt;domegadt];

end
function [ev,stop,dir] = dropoff(t,w,R,g)

theta = w(1);  omega = w(2);
N = R*omega^2 + g*cos(theta);
ev = N;
stop = 1;
dir = 0;

end
