% Written By: Christina Morrow, Ryker Mullinix
% Date: 2/3/2021
% Email: cjm18bn@my.fsu.edu, rwm18c@my.fsu.edu

% FAMU-FSU College of Engineering
% Senior Design Team 517

% Description: This program generates 

%% Assumptions
% length
% width
% height
% etc

clc; clear all; close all;

%% problem Set-Up

kingpost_length = 557/1000;    % length of LSMS kingpost [m]
upperarm = 1030/1000;   % length of LSMS upperarm [m] 
forearm_length = 994/1000;     % length of LSMS forearm [m] 

r_full_extend = kingpost_length + upperarm + forearm_length;  % full extension

f = 25*4.448;                 % force due to the LSMS [Newtons]
tau_worst = r_full_extend*f;  % torque given full extension of LSMS 

disp("Output Torque:")
fprintf("\t 1) LSMS is fully extended: %5.2f [Nm]\n",tau_worst)
% fprintf("\t 2) turntable is rotated 45 [deg]: %5.2f [Nm]\n",tau_rotate_45)
disp(" ")

%% Compound Gear Calculations

% Gear info
pitch = 20;
pressureAngle = 14.5;   % [degrees]
shaftDiam = 3/8;        % [inch]

% current gears:
% https://www.mcmaster.com/6867K37/
% https://www.mcmaster.com/6867K27/

% gear number of teeth
A = 15;
B = 60;
C = 15;
D = 60;

% driven (big) / driver (little)
GR = (B*D)/(A*C);                   % gear ratio
MA = (B/A) + (D/C);                 % mechanical adantage
fprintf("Gear Ratio: %2.1f\n",GR)
fprintf("Mechanical Advantage: %2.1f\n",MA)

motorTorque = ((tau_worst/GR));
fprintf("Input Motor Torque: %2.1f [Nm]\n",motorTorque)

moon = 30*1.625;
earth = 30*9.81;

tau_king = 5.49*36.72*0.113; % [in-lbs]
tau_shoulder_min = 3.023*20.275*0.113; % [in-lbs]

% model forearm as a cylinder

%% Ryker Calculations:
clc; clear all; close all;

% constants variables:
moon_gravity_coef = 1.625;      % [m/s^2]

% lengths of all LSMS links
forearm_length = 994/1000;      %[mm]
shoulder_length = 1030/1000;    %[mm]
kingpost_length = 557/1000;     %[mm]
length_tot = forearm_length+shoulder_length+kingpost_length; % [mm]

% distance to center of gravity
shoulder_r = 0.514985;          %[m]
kingpost_r = 0.932688;          %[m]

factor = 25/length_tot; % multiplication factor for weight

% forearm_mass = forearm_length*factor;   %[kg]
% shoulder_mass = shoulder_length*factor; %[kg]
% kingpost_mass = kingpost_length*factor; %[kg]

forearm_mass = 5.5836; %[kg]
shoulder_mass = 6.8969; %[kg]
kingpost_mass = 12.519; %[kg]

% model forearm as a cylinder
% assuming aluminum 7075
rho = 2810;     % density of al7075 in [kg/m^3]
t = 5;          % [mm]

forearm_mass_new = 1.158121*10^-2*1000; % [kg]
forearm_r = 34.5/1000; % [mm] to [m]

tau_forearm_min = forearm_mass_new*moon_gravity_coef*forearm_r; % [Nm]
tau_shoulder_min = shoulder_mass*moon_gravity_coef*shoulder_r;  % [Nm]
tau_kingpost_min = kingpost_mass*moon_gravity_coef*kingpost_r;  % [Nm]

fprintf("Min Torque Forearm: %4.2f [Nm]\n",tau_forearm_min)
fprintf("Min Torque Shoulder: %4.2f [Nm]\n",tau_shoulder_min)
fprintf("Min Torque Kingpost: %4.2f [Nm]\n\n",tau_kingpost_min)

tau_min = tau_forearm_min+tau_shoulder_min+tau_kingpost_min;    % [Nm]
fprintf("Min Torque: %2.4f [Nm]\n\n",tau_min)

kingpost_r_max = shoulder_length + 0.5*kingpost_length;

tau_forearm_max = forearm_mass_new*moon_gravity_coef*forearm_r;     % [Nm]
tau_shoulder_max = shoulder_mass*moon_gravity_coef*shoulder_r;      % [Nm]
tau_kingpost_max = kingpost_mass*moon_gravity_coef*kingpost_r_max;  % [Nm]

fprintf("Max Torque Forearm: %4.2f [Nm]\n",tau_forearm_max)
fprintf("Max Torque Shoulder: %4.2f [Nm]\n",tau_shoulder_max)
fprintf("Max Torque Kingpost: %4.2f [Nm]\n\n",tau_kingpost_max)

tau_max = tau_forearm_max+tau_shoulder_max+tau_kingpost_max;        % [Nm]
fprintf("Max Torque: %2.4f [Nm]\n\n",tau_max)

%% Motor

torque = 33/3; % [Nm]
radius = (2.38/2)/39.37; % [m]

force = torque/radius;

