%  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 lagrange_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

%  Simulation parameters:

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

psidot0 = 1*(2*pi);             %  rad/s
thetadot0 = 0*(2*pi);           %  rad/s
phidot0 = 4*(2*pi);             %  rad/s
psi0 = 0*(pi/180);              %  rad
theta0 = 30*(pi/180);           %  rad
phi0 = 0*(pi/180);              %  rad

Y0 = [psidot0, thetadot0, phidot0, psi0, theta0, phi0]';

%  Plotting parameters:

span = [0.8, 1.2];

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

% psidot0 = 1*(2*pi);         %  rad/s
% theta0 = 30*(pi/180);       %  rad
% 
% phidot0 = ((lambdat - lambdaa + m*L3^2)* ...                    %  rad/s
%             cos(theta0)*psidot0^2 + m*g*L3)/(lambdaa*psidot0);
%
% Y0 = [psidot0, 0, phidot0, 0, theta0, 0]';

%  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);
F = @(t, Y) F(t, Y, m, g, lambdat, lambdaa, L3);

%  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

%  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 and plot the location of the mass center over time:

x1 = L3*sin(psi).*sin(theta);       	%  m
x2 = -L3*cos(psi).*sin(theta);          %  m
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)')
ylim([min(min(x1*100)*span), max(max(x1*100)*span)])
subplot(312)
plot(t, x2*100, '-r', 'linewidth', 2)
xlabel('Time (s)')
ylabel('\itx\rm_{2} (cm)')
ylim([min(min(x2*100)*span), max(max(x2*100)*span)])
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 over time the top's total mechanical energy and 
%  components of the angular momentum about the fixed contact point P:

x1dot = L3*cos(psi).*sin(theta).*psidot + ...       %  m/s
        L3*sin(psi).*cos(theta).*thetadot;         
x2dot = L3*sin(psi).*sin(theta).*psidot - ...       %  m/s
        L3*cos(psi).*cos(theta).*thetadot;        
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;

HPE3 = lambdat*omega1.*sin(theta).*sin(phi) + ...       	%  kg-m^2
       lambdat*omega2.*sin(theta).*cos(phi) + ...
       lambdaa*omega3.*cos(theta) + m*L3*x1dot.*sin(theta).*cos(psi) + ...
       m*L3*x2dot.*sin(theta).*sin(psi);     

HPe3 = 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, HPE3*(100^2), '-r', 'linewidth', 2)
xlabel('Time (s)')
ylabel({['Angular']; ['momentum (kg-cm^2)']})
legend(' \bf{H}\rm_{\itP\rm} \cdot \bf{E}\rm_{3}')
ylim([min(min(HPE3*(100^2))*span), max(max(HPE3*(100^2))*span)])  
subplot(313)
plot(t, HPe3*(100^2), '-k', 'linewidth', 2)
xlabel('Time (s)')
ylabel({['Angular']; ['momentum (kg-cm^2)']})
legend(' \bf{H}\rm_{\itP\rm} \cdot \bf{e}\rm_{3}')
ylim([min(min(HPe3*(100^2))*span), max(max(HPe3*(100^2))*span)]) 

%  Animate the motion of the top:

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