% This script combines a half-rise cycloidal (Fig. 6.22(a), eqns. (6.22)
% and a half-rise harmonic (Fig. 6.20(b), eqns. (6.19) into a single
% full-rise motion of 1 inch in pi radians (180 DEG).  Boundary conditions
% of matching velocity (y') and jerk (y''') plus overall lift and duration
% provide the four constraints necessary to solve for the four unknowns.
%
% The analytical solution for the durations (beta 1 & 2) and lifts (L1 &
% L2) were done in class.  The plots generated here should verify that
% analysis.

L = 1;          % Total rise distance (in)
RAD = pi/180;   % Radian conversion factor
DEG = 180/pi;   % Degree conversion factor
DTHETA = 1;     % Degree stepsize for final plots

% The following durations and lifts were found analytically; see the PDF
% document (if I ever get it typeset) for method of solution.

% Beta 2 (could have used beta 1) is actually one of the roots of a quadratic

beta2 = roots([1 2*pi -pi^2]);  % Get both roots, one is positive
beta2 = max(beta2);     % Correct root for beta2
beta1 = pi-beta2;

L1 = (pi*beta1)/(4*beta2+pi*beta1);	% Lift for segment 1 (in)
L2 = 1-L1;                          % Lift for segment 2 (in)

% First find the displacement, first and second kinematic coefficient
% functions for the half-rise cycloidal segment, termed segment 1.  Use 100
% points for each motion segment; this should yield a smooth plot.  Note
% that this will NOT be an "even degree" increment.

t1r = linspace(0,beta1,100)';	% Column array of 100 points from 0 -> beta1
t1d = t1r*DEG;                  % Convert to DEG

% First compute the displacement function y1 and plot it vs t1d (DEG)

y1 = L1*((t1r/beta1)-(1/pi)*sin(pi*t1r/beta1));
plot(t1d,y1,'k-','linewidth',2);
axis([0 beta1*DEG min(y1)*1.1 max(y1)*1.1]);
grid;
set(gca,'fontsize',20);
xlabel('Cam angle (DEG)','fontsize',20);
ylabel('Follower motion y (in)','fontsize',20);
title('Displacement Function for Segment 1','fontsize',20);
pause;
delete(gcf);

% Next the first kinematic coefficient y1p (y1p*omega = velocity ydot)

y1p = (L1/beta1)*(1-cos(pi*t1r/beta1));
plot(t1d,y1p,'k-','linewidth',2);
axis([0 beta1*DEG min(y1p)*1.1 max(y1p)*1.1]);
grid;
set(gca,'fontsize',20);
xlabel('Cam angle (DEG)','fontsize',20);
ylabel('1st Kin. Coeff. (VEL)','fontsize',20);
title('First Kinematic Coeff ("VEL") for Segment 1','fontsize',20);
pause;
delete(gcf);

% Finally the second kinematic coefficient y1pp (y1pp*omega^2 = accel yddot)

y1pp = (pi*L1/beta1^2)*sin(pi*t1r/beta1);
plot(t1d,y1pp,'k-','linewidth',2);
axis([0 beta1*DEG min(y1pp)*1.1 max(y1pp)*1.1]);
grid;
set(gca,'fontsize',20);
xlabel('Cam angle (DEG)','fontsize',20);
ylabel('2nd Kin. Coeff. (ACCEL)','fontsize',20);
title('Second Kinematic Coeff ("ACCEL") for Segment 1','fontsize',20);
pause;
delete(gcf);

% Now plot all three on the same plot

plot(t1d,y1,'k-','linewidth',2); hold on;
plot(t1d,y1p,'r-','linewidth',2);
plot(t1d,y1pp,'b-','linewidth',2);
axis([0 beta1*DEG min(y1p)*1.1 max(y1p)*1.1]);
legend('Displ','"Vel"','"Accel"','location','northwest');
grid;
set(gca,'fontsize',20);
xlabel('Cam angle (DEG)','fontsize',20);
ylabel('Displ, 1st & 2nd Coeff. (in)','fontsize',20);
title('Displ, "Vel", "Accel" for Segment 1','fontsize',20);
pause;
delete(gcf);

% Next find the displacement, first and second kinematic coefficient
% functions for the 2nd half-rise SHM segment.  This is termed Segment 2.
% Note that we start segment 2 from zero angle, even though it "really"
% starts at 90 degrees.  Much easier if you start from zero.

t2r = linspace(0,beta2,100)';	% Column rray of 100 points from 0 -> beta2
t2d = t2r*DEG;                  % Convert to DEG

% Same drill here; first compute the displacement function y2 and plot.
% I've added the distance L1 to y2 so it will end at the correct lift.

y2 = L1+L2*sin(pi*t2r/(2*beta2));
plot(t2d,y2,'k-','linewidth',2);
axis([0 beta2*DEG min(y2) max(y2)*1.1]);
grid;
set(gca,'fontsize',20);
xlabel('Cam angle (DEG)','fontsize',20);
ylabel('Follower motion y (in)','fontsize',20);
title('Displacement Function for Segment 2','fontsize',20);
pause;
delete(gcf);

% Next compute the first and second kinematic coefficients yp and ypp and
% plot

y2p = (pi*L2/(2*beta2))*cos(pi*t2r/(2*beta2));
plot(t2d,y2p,'k-','linewidth',2);
axis([0 beta2*DEG min(y2p)*1.1 max(y2p)*1.1]);
grid;
set(gca,'fontsize',20);
xlabel('Cam angle (DEG)','fontsize',20);
ylabel('1st Kin. Coeff. (VEL)','fontsize',20);
title('First Kinematic Coeff ("VEL") for Segment 2','fontsize',20);
pause;
delete(gcf);

y2pp = -(pi^2*L2/(4*beta2^2))*sin(pi*t2r/(2*beta2));
plot(t2d,y2pp,'k-','linewidth',2);
axis([0 beta2*DEG min(y2pp)*1.1 max(y2pp)*1.1]);
grid;
set(gca,'fontsize',20);
xlabel('Cam angle (DEG)','fontsize',20);
ylabel('2nd Kin. Coeff. (ACCEL)','fontsize',20);
title('Second Kinematic Coeff ("ACCEL") for Segment 2','fontsize',20);
pause;
delete(gcf);

% Now plot all three on the same plot

plot(t2d,y2,'k-','linewidth',2); hold on;
plot(t2d,y2p,'r-','linewidth',2);
plot(t2d,y2pp,'b-','linewidth',2);
axis([0 beta2*DEG min([min(y2) min(y2p) min(y2pp)])*1.1 ...
    max([max(y2) max(y2p) max(y2pp)])*1.1]);
legend('Displ','"Vel"','"Accel"','location','best');
grid;
set(gca,'fontsize',20);
xlabel('Cam angle (DEG)','fontsize',20);
ylabel('Displ, 1st & 2nd Coeff. (in)','fontsize',20);
title('Displ, "Vel", "Accel" for Segment 2','fontsize',20);
pause;
delete(gcf);

% Even though the minimum follower radius may have been computed, one can
% get that value from the numerical values just computed.  We want to
% find the maximum value of the magnitude of y1p (the max velocity will
% occur during Segment 1).

r_min = max(abs(y1p));  % Find the maximum of the absolute value of y1p

s = sprintf('\nMinimum follower radius is: %.2f inches\n', r_min); % Display
disp(s);

% Next find the minimum base circle radius.  We will find the minimum base
% circle radius so that the cam profile has no cusps, then give the user
% the option of entering the desired base circle radius.  For no cusps, the
% minimum base circle radius Ro is equal to the maximum value of (-y-ypp).
% Since we've computed both of those, finding the max should be easy.  We
% do need to check both segments 1 and 2, although intuitively segment 1
% has the fastest motion, so it should determine.

R(1) = max(-y1-y1pp);
R(2) = max(-y2-y2pp);

Ro = max(R);

s = sprintf('Minimum base circle radius is: %.4f inches\n', Ro); % Display
disp(s);

Ro = input('Enter desired base circle radius: ');

% Now we can calculate the cam profile using the xc and yc equations from
% the cam/follower geometric analysis.  However, this will require
% concatenating the two motions.  Whenever one does this, there will be a
% duplicate point between the two that should be removed.

N1 = length(t1r);   % Length of segment 1 (samples)
N2 = length(t2r);   % Length of segment 2 (samples)

% Now to "cement" the two segments together.  Since they are both column
% vectors, we can just stack them one on top of the other.  Before we do
% that, however, remove the last point from the first segment, since it's a
% duplicate

y1 = y1(1:N1-1);        % Remove the last point from 
y1p = y1p(1:N1-1);      % Segment 1 for all three
y1pp = y1pp(1:N1-1);	% motion functions

t1r = t1r(1:N1-1);  % Same thing for theta (RAD)

% Now stack them on top of one another

y = [y1;y2];    % The ";" operator means new row, so the two cols are stacked
yp = [y1p;y2p];
ypp = [y1pp;y2pp];
thetar = [t1r;t2r+beta1];

% Now plot the entire motion: displacement, "velocity", and "acceleration"

plot(thetar*DEG,y,'k-','linewidth',2);
axis([0 180 min(y)*1.1 max(y)*1.1]);
grid;
set(gca,'fontsize',20);
xlabel('Cam angle (DEG)','fontsize',20);
ylabel('Follower motion y (in)','fontsize',20);
title('Displacement Function for Entire Motion','fontsize',20);
pause;
delete(gcf);

plot(thetar*DEG,yp,'k-','linewidth',2);
axis([0 180 min(yp)*1.1 max(yp)*1.1]);
grid;
set(gca,'fontsize',20);
xlabel('Cam angle (DEG)','fontsize',20);
ylabel('1st Kin. Coeff. (VEL)','fontsize',20);
title('First Kinematic Coeff ("VEL") for Entire Motion','fontsize',20);
pause;
delete(gcf);

plot(thetar*DEG,ypp,'k-','linewidth',2);
axis([0 180 min(ypp)*1.1 max(ypp)*1.1]);
grid;
set(gca,'fontsize',20);
xlabel('Cam angle (DEG)','fontsize',20);
ylabel('2nd Kin. Coeff. (ACCEL)','fontsize',20);
title('Second Kinematic Coeff ("ACCEL") for Entire Motion','fontsize',20);
pause;
delete(gcf);

% This is enough.  The parameters can now be passed to the camprofile.m
% function, which will produce the (xc,yc) profile points.  These can
% easily plotted.

disp('Arrays y, yp, theta created...ready for camprofile.m to');
disp('generate (x,y) coordinates of cam profile.');
disp(' ');  % Blank line