
% some of the values that are set in this file are not used until later 
% parts of the problem

global r r2 rg lthg;
global iwk iwu1 iwu2 ium;
global w_data;
global gc g_by_gc; 
global m ai;
global l_statics;

% rated torque of motor
max_motor_torque_output = 6.0;

% do as dynamics problem
l_statics = 0;

% figure counter
ifig = 0;

% some of geometry of linkage
%
% lengths of bars in linkage (meters)
r = 0.01*[10.0 40.0 30.0 40.0]';
r2 = r*r';
% angle (relative to local coordinates on link) to the center of mass;
% see m-file linkage_ag.m
lthg = [0.0 0.0 pi/6]';
% distance to centers of mass from starting point of link
rg = [r(1)*0.5 r(2)*0.5 r(3)/cos(lthg(3))]';

% masses, moments of inertia
m = [0.0 0.0 10.0]'; % in kg
% moment of inertia for uniform distribution of mass over links
ai = zeros(3,1);
ai(1) = m(1)*r2(1,1)/12.0;
ai(2) = m(2)*r2(2,2)/12.0;
diam_fan = 1.0;
ai(3) = m(3)*diam_fan*diam_fan/16.0;

% gravitation
g       = [0.0 -9.8]'; % direction 2 is down, in m/s^2
gc      = 1.0; % metric, trust mass to be in kg
g_by_gc = g/gc;

th1_init = atan2(3,4);
th_init = fan_angles(r,r2,th1_init);

% data for initial angular velocities
w_data = [2.0*pi 0.0 0.0]';
% indices for known (iwk) and unknown (iwu*) angular velocities
% and index for unknown moment (ium)
iwk = 1; iwu1 = 2; iwu2 = 3; ium = 1;

tstart = 0.0; tstop = 2.0*pi/abs(w_data(iwk)); nt = 101;
tspan = linspace(tstart, tstop, nt);
dtime = (tstop - tstart)/(nt-1);


options = odeset('AbsTol',1e-10);
[t, th_num] = ode45('fan_solve',tspan,th_init,options);



% obtain accelerations over a full cycle

thdot_all = zeros(nt,3);
ag_fan = zeros(nt,2);   mag_ag_fan = zeros(nt,1);
thddot_fan = zeros(nt); mag_thddot_fan = zeros(nt,1);
for it = 1:nt;
   [thdot,thddot,ag] = fan_solve(t(it), th_num(it,:));
   % store results at time it
   ag_fan(it,:) = ag(:,3)';
   mag_ag_fan(it) = norm(ag(:,3));
   thddot_fan(it) = thddot(3);
   mag_thddot_fan(it) = abs(thddot(3));
   thdot_all(it,:) = thdot';
end;

% plot all of the angular velocities of the moving links
ifig = ifig+1; figure(ifig);
plot(...
   t, thdot_all(:,1), ':', ...
   t, thdot_all(:,2), '-', ...
   t, thdot_all(:,3), '--')
xlabel('time (s)'); ylabel('angular velocity (radians/s)');
title('angular velocities of links, from numerical integration');
legend('thdot1','thdot2','thdot3');

% plot angular acceleration of link 3

ifig = ifig+1; figure(ifig);
plot(t,thddot_fan);
xlabel('time (s)'); ylabel('angular acceleration (rad/s^2)');
title('angular acceleration of fan for a full cycle');

% plot componenets of linear acceleration of fan's center of mass over cycle

ifig = ifig+1; figure(ifig);
plot(...
   t,ag_fan(:,1), '-', ...
   t,ag_fan(:,2), '--');
xlabel('time (s)'); ylabel('linear acceleration (m/s^2)');
title('components of acceleration of fan for a full cycle');
legend('x component','y component');



% loop over cycle to get moment at motor
ma = zeros(nt,1);
for it = 1:nt;
	[thdot,thddot,ag,f] = fan_solve(t(it), th_num(it,:));
	ma(it) = f(9);
end;

% check of conservation of energy over a cycle, scale the integral
% by the integral of the absolute value (to get the error as a 
% relative magnitude)
%
integ = sum(ma(1:(nt-1)))*dtime*w_data(iwk);
integ_scale = sum(abs(ma(1:(nt-1))))*w_data(iwk);
fprintf('check of energy conservation %.3e\n', ...
	integ/integ_scale);

if (w_data(iwk) > 0.0);
  % moment is in same direction as rotation so work flows into
  % the system when the moment is positive
  sign_wk = sign(w_data(iwk));
else
  % moment is in opposite direction as rotation so work flows into
  % the system when the moment is negative
end

% moment at motor
ifig = ifig+1; figure(ifig);
plot(t,ma*sign_wk);
xlabel('time (s)'); ylabel('moment (N/m)');
title('moment for a full cycle; when positive, work flows into system');

max_moment_by = max( ma*sign_wk);
max_moment_on = max(-ma*sign_wk);
%
fprintf('maximum moment exerted by motor %.3f\n',max_moment_by);
fprintf('maximum moment exerted on motor %.3f\n',max_moment_on);

if (max_moment_by > max_motor_torque_output); 
  fprintf('the motor is not capable of driving the mechanism over a cycle\n');
else
  fprintf('the motor is capable of driving the mechanism over a cycle\n');
end

% now check and see if the motor could have driven the fan if we had just
% done statics (not required, but fun to check)

l_statics = 1;
%
% see fan_solve function for the solution this week; all of the kinematics
% are the same, just have to rerun the force and moment calculation
ma_statics = zeros(nt,1);
for it = 1:nt;
	[thdot,thddot,ag,f] = fan_solve(t(it), th_num(it,:));
	ma_statics(it) = f(9);
end;

% moment at motor, statics
ifig = ifig+1; figure(ifig);
plot(t,ma_statics*sign_wk);
xlabel('time (s)'); ylabel('moment (N/m)');
title('moment for a full cycle; statics; when positive, work flows into system');

max_moment_by_statics = max( ma_statics*sign_wk);

fprintf('for the case of statics ...\n');
if (max_moment_by_statics > max_motor_torque_output); 
  fprintf('the motor is not capable of driving the mechanism over a cycle\n');
else
  fprintf('the motor is capable of driving the mechanism over a cycle\n');
end

