% fourbarpos evalutaion script
% evaluates fourbarpos.m
% copied from HW2_P3_fourbarpos_eval
% modified for Final Project Finger Wagging Mechanism
% 
% 
% last modified 28 Apr 2023
% 28 Apr: created

close all;

% link lengths
l = [32, 224, 160, 160];
%   [ a ,   b ,   c ,   d ]
%   [L_2,  L_3,  L_4,  L_1]
%   [O2-A, A-B, B-O4, O2-O4]

% angles
theta1 = 0;  % typically zero
% theta2 = 40;  % not used; sweeping thru theta2

% open or closed chain circuit
delt = -1;

% toggle angle
toggle_angle = 116.0371;

% calculate values
Theta2 = -toggle_angle:1:toggle_angle;
inc = length(Theta2);
Theta31 = zeros(1,inc);
Theta41 = zeros(1,inc);
Theta32 = zeros(1,inc);
Theta42 = zeros(1,inc);
P_x1 = zeros(1,inc);  % x-component of point P WRT grounded link
P_y1 = zeros(1,inc);  % y-component of point P WRT grounded link
P_x2 = zeros(1,inc);  % Toggle 2
P_y2 = zeros(1,inc);  % Toggle 2
P_l = 1.33;          % distance of P from point A
P_angle = 0;         % [deg], angle of point P from theta3
a = l(1);            % unpack link "a" (L_2)
d = l(4);            % unpack link "d" (L_1)

% first toggle position
for angle = 1:inc
    theta2 = Theta2(angle);
    angles = fourbarpos(l, theta1, theta2, delt);

    Theta31(angle) = angles(3);
    Theta41(angle) = angles(4);

    P_x1(angle) = a*cosd(theta2) + P_l*cosd(angles(3) + P_angle);
    P_y1(angle) = a*sind(theta2) + P_l*sind(angles(3) + P_angle);

% calculate the other toggle position
    angles = fourbarpos(l, theta1, theta2, -delt);

    Theta32(angle) = angles(3);
    Theta42(angle) = angles(4);

    P_x2(angle) = a*cosd(theta2) + P_l*cosd(angles(3) + P_angle);
    P_y2(angle) = a*sind(theta2) + P_l*sind(angles(3) + P_angle);
end

% plot values
[Theta31_max, index] = max(Theta31)  % callout max value
Theta2_max = Theta2(index);
[Theta31_min, index] = min(Theta31)  % callout min value
Theta2_min = Theta2(index);
plot(Theta2, Theta31, Theta2_max, Theta31_max, 'ro', Theta2_min, Theta31_min, 'ro')
% plot(Theta2, Theta31),
legend('Theta 3')
xlabel('Theta 2'), ylabel('Angular Displacement')
title('Output Angle vs. Input Angle')