%  Daniel Kawano, Rose-Hulman Institute of Technology
%  Last modified:  Feb 10, 2016

clear all
close all
clc

%  Load the symbolic function handles for the state equations in
%  mass-matrix form, M(t,Y)*Y'(t) = F(t,Y):

load poisson_top_ODEs

%  Physical parameters:

m = 8/1000;                     %  kg
g = 9.81;                       %  m/s^2
r = 20/1000;                    %  m
lambdat = 1/4*m*r^2;            %  kg-m^2
lambdaa = 1/2*m*r^2;            %  kg-m^2
L3 = 20/1000;                   %  m
muk = 0.010;

%  Simulation parameters:

dt = 0.001;                 	%  s
tf = 8;                         %  s
tsim = [0 : dt : tf]';        	%  s    
tol = 1e-6;

%  Initial conditions for steady motion of the top if the surface is
%  smooth.  Specify the rate of precession and the nutation angle to 
%  determine the required spin rate:

psidot0 = 1*(2*pi);             %  rad/s
theta0 = 10*(pi/180);           %  rad

phidot0 = ((lambdat - lambdaa)*cos(theta0)*psidot0^2 + ...      %  rad/s
           m*g*L3)/(lambdaa*psidot0);

x10 = 0/100;          	%  m
x20 = 0/100;         	%  m 
x1dot0 = 0/100;         %  m/s
x2dot0 = 9/100;         %  m/s     

Y0 = [psidot0, 0, phidot0, 0, theta0, 0, x1dot0, x2dot0, x10, x20]';

%  Plotting parameters:

span = [0.8, 1.2];

%  Convert M(t,Y) and F(t,Y) to purely numeric function handles for
%  numerical integration:

M = @(t, Y) M(t, Y, m, g, lambdat, lambdaa, L3, muk);
F = @(t, Y) F(t, Y, m, g, lambdat, lambdaa, L3, muk);

%  Numerically integrate the state equations:

options = odeset('mass', M, 'abstol', tol, 'reltol', tol);

[t, Y] = ode45(F, tsim, Y0, options);

%  Extract the results:

psidot = Y(:,1);            %  rad/s
thetadot = Y(:,2);          %  rad/s
phidot = Y(:,3);            %  rad/s
psi = Y(:,4);               %  rad
theta = Y(:,5);             %  rad
phi = Y(:,6);               %  rad
x1dot = Y(:,7);             %  m/s
x2dot = Y(:,8);             %  m/s
x1 = Y(:,9);                %  m
x2 = Y(:,10);               %  m

%  Plot the precession rate, nutation angle, and spin rate over time:

figure
set(gcf, 'color', 'w')
subplot(311)
plot(t, psidot/(2*pi), '-b', 'linewidth', 2)
xlabel('Time (s)')
ylabel('Precession rate (rev/s)')
ylim([min(min(psidot/(2*pi))*span), max(max(psidot/(2*pi))*span)])
subplot(312)
plot(t, theta*(180/pi), '-r', 'linewidth', 2)
xlabel('Time (s)')
ylabel('Nutation angle (deg)')
ylim([min(min(theta*(180/pi))*span), max(max(theta*(180/pi))*span)])
subplot(313)
plot(t, phidot/(2*pi), '-k', 'linewidth', 2)
xlabel('Time (s)')
ylabel('Spin rate (rev/s)')
ylim([min(min(phidot/(2*pi))*span), max(max(phidot/(2*pi))*span)])

%  Calculate the vertical position of the top's mass center, and then 
%  plot the mass center location over time: 

x3 = L3*cos(theta);         %  m
 
figure
set(gcf, 'color', 'w')
subplot(311)
plot(t, x1*100, '-b', 'linewidth', 2)
xlabel('Time (s)')
ylabel('\itx\rm_{1} (cm)')
subplot(312)
plot(t, x2*100, '-r', 'linewidth', 2)
xlabel('Time (s)')
ylabel('\itx\rm_{2} (cm)')
subplot(313)
plot(t, x3*100, '-k', 'linewidth', 2)
xlabel('Time (s)')
ylabel('\itx\rm_{3} (cm)')
ylim([min(min(x3*100)*span), max(max(x3*100)*span)])

%  Calculate and plot the speed of the top's contact point P with the
%  ground over time:

vP1 = x1dot - L3*cos(psi).*sin(theta).*psidot - ...   	%  m/s
      L3*sin(psi).*cos(theta).*thetadot;
vP2 = x2dot - L3*sin(psi).*sin(theta).*psidot + ...   	%  m/s
      L3*cos(psi).*cos(theta).*thetadot;
vP = sqrt(vP1.^2 + vP2.^2);                         	%  m/s

figure
set(gcf, 'color', 'w')
plot(t, vP*100, '-b', 'linewidth', 2)
xlabel('Time (s)')
ylabel('Contact point speed (cm/s)')
ylim([min(min(vP*100)*span), max(max(vP*100)*span)])

%  Calculate and plot over time the top's total mechanical energy and 
%  components of the angular momentum about the mass center:

x3dot = -L3*sin(theta).*thetadot;       %  m/s

omega1 = psidot.*sin(theta).*sin(phi) + thetadot.*cos(phi);     %  rad/s
omega2 = psidot.*sin(theta).*cos(phi) - thetadot.*sin(phi);     %  rad/s
omega3 = psidot.*cos(theta) + phidot;                           %  rad/s

E = 1/2*m*(x1dot.^2 + x2dot.^2 + x3dot.^2) + ...            %  J
    1/2*(lambdat*omega1.^2 + lambdat*omega2.^2 + ...
         lambdaa*omega3.^2) + m*g*x3;

HE3 = lambdat*omega1.*sin(theta).*sin(phi) + ...            %  kg-m^2
      lambdat*omega2.*sin(theta).*cos(phi) + ...
      lambdaa*omega3.*cos(theta);
     
He3 = lambdaa*omega3;           %  kg-m^2                    

figure
set(gcf, 'color', 'w')
subplot(311)
plot(t, E*1000, '-b', 'linewidth', 2)
xlabel('Time (s)')
ylabel({['Total mechanical']; ['energy (mJ)']})
ylim([min(min(E*1000)*span), max(max(E*1000)*span)])
subplot(312)
plot(t, HE3*(100^2), '-r', 'linewidth', 2)
xlabel('Time (s)')
ylabel({['Angular']; ['momentum (kg-cm^2)']})
legend(' \bf{H}\rm \cdot \bf{E}\rm_{3}')
ylim([min(min(HE3*(100^2))*span), max(max(HE3*(100^2))*span)])  
subplot(313)
plot(t, He3*(100^2), '-k', 'linewidth', 2)
xlabel('Time (s)')
ylabel({['Angular']; ['momentum (kg-cm^2)']})
legend(' \bf{H} \cdot \bf{e}\rm_{3}')
ylim([min(min(He3*(100^2))*span), max(max(He3*(100^2))*span)])  
        
%  Animate the motion of the top:

animate_poisson_top(L3, psi, theta, phi, x1, x2, x3, dt, 0/100);