8) Appendix [ATD]

clear all;
close all;

% program settings
plot_container_path = false;
plot_distribution_points = false;
plot_distribution_circles = true;
show_labels = false;
show_linkages_and_container = false;
write_to_video = true;

% physical params 
D = 190; % [mm]
d = 45.51; % [mm]
dist_slider_pt_to_container_ctr = 101.43; % [mm]
dist_crank_ctr_to_dish_ctr = 225; % [mm]
length_crank = 27.5; % [mm]
length_connecting_link = 167.5; % [mm]

% animated curve of path (stored in polar coordinates, with r referring to
% y position of the topping container and theta referring to
% theta_turntable)
path_points = struct('r', [], 'theta', []);

figure;
set(gcf,'Color','w');

for theta_crank = linspace(0, 2*pi, 200)

    clf;
    axis equal;
    hold on;
    set(gca,'Visible','off');
    
    % set size of plot
    center = struct('x', 0, 'y', -80);
    span = struct('x', 400, 'y', 400);
    xlim([(center.x - span.x/2) (center.x + span.x/2)]);
    ylim([(center.y - span.y/2) (center.y + span.y/2)]);
    
    % draw a circle for the dish
    theta_turntable = theta_crank*6;
    circleWithTicks(0, 0, D, theta_turntable, 'b', 'EdgeColor', 'b', 'LineStyle', '-', 'LineWidth',2);
%     plot(0, 0, 'b.');
    if show_labels
        text(75, 75, 'D', 'FontSize', 14, 'Color', 'b');
    end
    
    % draw the crank
    crank_start_pt = struct('x', 0, 'y', -dist_crank_ctr_to_dish_ctr);
    crank_end_pt = struct('x', crank_start_pt.x+length_crank*cos(theta_crank), 'y', crank_start_pt.y+length_crank*sin(theta_crank));
    if show_linkages_and_container
        plot([crank_start_pt.x crank_end_pt.x], [crank_start_pt.y, crank_end_pt.y], 'm-', 'LineWidth',2);
    end
    
    % draw the connecting link
    connecting_link_start_pt = crank_end_pt;
    connecting_link_end_pt = struct( ...
        'x', crank_start_pt.x, ...
        'y', connecting_link_start_pt.y+length_connecting_link*sin(acos(-crank_start_pt.x/length_connecting_link)) ...
        );
    if show_linkages_and_container
        plot([connecting_link_start_pt.x connecting_link_end_pt.x], [connecting_link_start_pt.y, connecting_link_end_pt.y], 'g-', 'LineWidth',2);
    end

    % draw the distance from the topping container to the end of the long
    % arm of the crank slider
    if show_linkages_and_container
        plot([0 0], [connecting_link_end_pt.y, connecting_link_end_pt.y-d/2+dist_slider_pt_to_container_ctr],'Color','#4DBEEE','LineStyle',':','LineWidth',2);
    end

    % draw joints
    if show_linkages_and_container
        plot(crank_start_pt.x, crank_start_pt.y, 'k.', 'MarkerSize', 10);
        plot(crank_end_pt.x, crank_end_pt.y, 'k.', 'MarkerSize', 10);
        plot(connecting_link_end_pt.x, connecting_link_end_pt.y, 'k.', 'MarkerSize', 10);
    end
    
    % calculate the position of the container
    container_y = connecting_link_end_pt.y+dist_slider_pt_to_container_ctr;

    % add a point to the path_points of the current 
    path_points.r = [path_points.r container_y];
    path_points.theta = [path_points.theta theta_turntable+pi/2];
    % convert from polar to cartesian and plot
    [path_points_x, path_points_y] = pol2cart(-path_points.theta+pi+theta_turntable, path_points.r);
    if plot_container_path
        plot(path_points_x, path_points_y, "k-.");
    end

    % draw in topping coverage as it dispenses
    % build up array of dispensing coordinates
    distribution_spots = struct('r', [], 'theta', []);
    prev_rem = -1;
    for i = 1:length(path_points.theta)
        rem = mod(path_points.theta(i), pi/3);
        if rem < prev_rem
            distribution_spots.theta = [distribution_spots.theta path_points.theta(i)];
            distribution_spots.r = [distribution_spots.r path_points.r(i)];
        end
        prev_rem = rem;
    end
    % plot distribution spots
    [distribution_spots_x, distribution_spots_y] = pol2cart(-distribution_spots.theta+pi+theta_turntable, distribution_spots.r);
    if plot_distribution_points
        plot(distribution_spots_x, distribution_spots_y, "k*");
    end
    if plot_distribution_circles
        for p = 1:length(distribution_spots_x)
            circle(distribution_spots_x(p), distribution_spots_y(p), d, 'EdgeColor', 'none', 'FaceColor', [[1 1 0], .3])
        end
    end

    % draw the circle for the container (has to be on top of other plots)
    if show_linkages_and_container
        circle(0, container_y, d, 'EdgeColor', 'r', 'FaceColor', [[1 0 0], .3], 'LineWidth',2);
    end
    if show_labels
        text(d*.5, container_y+d*.5, 'd', 'FontSize', 14, 'Color', 'r');
    end

    pause(.03);
    frames(i) = getframe(gcf);  % capture the current frame

end

% write to video
if write_to_video
    writerObj = VideoWriter('animation1.mp4', 'MPEG-4');
    writerObj.FrameRate = 30;
    open(writerObj); 
    for i = 1:length(frames)
        writeVideo(writerObj, frames(i));
    end
    close(writerObj);
end


function circle(x, y, diam, varargin)
    rectangle('Position',[(x-diam/2) (y-diam/2) diam diam],'Curvature',[1 1], varargin{:});
end

function circleWithTicks(x, y, diameter, rotation, tick_color, varargin)
    radius = diameter/2;
    rectangle('Position',[(x-radius) (y-radius) diameter diameter],'Curvature',[1 1], varargin{:});
    for theta = linspace(0, 2*pi, 6)+rotation
        plot([x+(radius-5)*cos(theta), x+radius*cos(theta)], [y+(radius-5)*sin(theta), y+radius*sin(theta)], "-", 'Color', tick_color, 'LineWidth',2);
    end
end

close all;

% set omega_a to arbitrary value (in reality this is controlled by the motor)
omega_a = 10; % [rad/s]

% create linspace for theta_a
theta_a = linspace(0, 2*pi, 200);

% physical attributes
a = 27.5; % [mm]
b = 167.5; % [mm]
dist_slider_pt_to_container_ctr = 101.43; % [mm]
dist_crank_ctr_to_dish_ctr = 225; % [mm]

% equations to find desired quantities
omega_b = (-a*omega_a*sin(theta_a))./(b*sin(acos(-a*cos(theta_a)/b)));
c_dot = -a*omega_a*cos(theta_a) - (a^2*omega_a*sin(omega_a).*cos(omega_a))./(b*sin(acos(-a*cos(theta_a)/b)));
c = (a*sin(theta_a) + b*sin(acos(-a*cos(theta_a)/b)))-dist_crank_ctr_to_dish_ctr+dist_slider_pt_to_container_ctr;

% create a nondimensional time variable that is a percentage of a full cycle
% of the crank
t = theta_a/(2*pi)*100;

% plot omega_b
figure;
plot(t, omega_b, "r-");
xlabel("% of full crank rotation cycle");
ylabel("$\omega_b$ (rad/s)", 'interpreter','latex');
title("Angular Velocity of Connecting Link");
subtitle("For \omega_a = 10 rad/s");

% plot c_dot
figure;
plot(t, c_dot, "b-");
xlabel("% of full crank rotation cycle");
ylabel("$\dot c$ (mm/s)", 'interpreter','latex');
title("Velocity of Container");
subtitle("For \omega_a = 10 rad/s");

% plot y
figure;
plot(t, c, "-");
xlabel("% of full crank rotation cycle");
ylabel("$r$ (mm)", 'interpreter','latex');
title("Radial Position of Container (Distance From Center)");