clc
clear all
close all
 
% Motor Max Operatable Torque
polulu_T_stall = 5; %oz-in
polulu_rpm = 11000; %RPM
polulu_T_op = polulu_T_stall/2;
polulu_gearbox = 131;
 
polulu_T_op_geared = polulu_T_op * polulu_gearbox
 
% Our Gearbox
teamGbox = 40/16;
T_op = polulu_T_op_geared * teamGbox;
 
% Convert to N-m
Nm_over_Ozin = 141.6119;
T_op_Nm = T_op/Nm_over_Ozin;
 
% Lenghts in inches
a = 3.15;
b= 6.89;
c = 6.89;
d = 8.94;
f = c + 6.89;
 
% inches to meters
i_to_m = 0.0254;
 
% L is in meters
L1 = a * i_to_m;
L2 = c * i_to_m;
L3 = b * i_to_m;
L4 = d * i_to_m;
LP = f * i_to_m;
qP = degtorad(0);%*pi/180;
 
xaxis_max = 0.46%0.2;
xaxis_min = -0.46;
yaxis_max = 0.48;
yaxis_min = -0.2;
 
m = 1; %1kg
g = 9.81; %Force Exerted by load N-m
 
alpha = degtorad(45)%1* pi/180;
% Rotation Matrix
R = [cos(alpha) -sin(alpha); sin(alpha) cos(alpha)]
 
dir = -1 % +1 or -1
% in degrees
qstart = -80;
qend = qstart+360; 
 
w2 = dir*100*2*pi/60; %RPM to rad/s
 
%video
writerObj = VideoWriter('4barAnim.avi');
 
open(writerObj);
for index = qstart:1:qend    
    i = (index-qstart)+1;
    q2 = dir*index;  % Degree
    q2 = dir*index*pi/180;  % Radian
    
    q2_i(i) = q2;
 
    K1 = L4 / L1;
    K2 = L4 / L3;
    K3 = (L1^2 - L2^2 + L3^2 +L4^2) / (2*L1*L3);
    K4 = L4 / L2;
    K5 = (L3^2 - L4^2 - L1^2 - L2^2) / (2*L1*L2);
 
    A = cos(q2) - K1 - K2*cos(q2) + K3;
    B = -2*sin(q2);
    C = K1 - (K2 + 1)*cos(q2) + K3;
    D = cos(q2) - K1 + K4*cos(q2) + K5;
    E = -2*sin(q2);
    F = K1 + (K4 - 1)*cos(q2) + K5;
 
    q4 = 2*atan((-B - sqrt(B^2 - 4*A*C)) / (2*A));
    q3 = 2*atan((-E - sqrt(E^2 - 4*D*F)) / (2*D));
 
    w3 = a/b*w2*sin(q4-q2)/sin(q3-q4);
    
    w3_i(i) = w3;
 
    % Calculate Instant Center w3_w1;
    I = [cos(q2), -cos(q4); sin(q2), -sin(q4)];
    LI = inv(I)*[L4;0];   
    
    origin = [0; 0];
    L_A = [L1*cos(q2); L1*sin(q2)];
    L_B = L_A + [L2*cos(q3); L2*sin(q3)];
    L_D = [L4*cos(0); L4*sin(0)];
    L_C = L_D + [L3*cos(q4); L3*sin(q4)];
    P = L_A + [LP*cos(q3 - qP); LP*sin(q3 - qP)];
    
    L_AI = [LI(1)*cos(q2); LI(1)*sin(q2)];
    L_DI = L_D + [LI(2)*cos(q4); LI(2)*sin(q4)];
    
    % Plot Point P
    vA = L1*w2*[-sin(q2); cos(q2)];
    vPA = LP*w3*[-sin(q3-qP); cos(q3-qP)];    
    vP = vA+vPA;
    
    % Force Direction
    F_load = [0;-m*g;0];
    % Arm Vector From Point I to Point P
    R_load = P-L_AI;      
    
 
    L_A = R*L_A;
    L_B = R*L_B;
    L_C = R*L_C;
    L_D = R*L_D;
    P = R*P;
    vP = R*vP;
    L_AI = R*L_AI;
    L_DI = R*L_DI;    
    R_load = R*R_load;
    % Torque of Load on Point B
    T_Load = cross([R_load(1), R_load(2),0], F_load);
    
    T_Load_i(i) = T_Load(3);
    
    vP_i(i) = -vP(1);%norm(vP,2);       
    vP_unit = vP/norm(vP,2);    
 
    Px(i) = P(1);
    Py(i) = P(2);
    
    figure(1)
    plot(origin(1),origin(2),'ro','linewidth',3)
    hold on
    plot(-L_A(1),L_A(2),'ro','linewidth',2)
    plot(-L_B(1),L_B(2),'ro','linewidth',2)
    plot(-L_D(1),L_D(2),'ro','linewidth',3)
    %plot(L_C(1),L_C(2),'bx','linewidth',2)
    plot(-P(1),P(2),'ro','linewidth',4)
    
    % Plot Instant Center I1,3
    plot(-L_AI(1),L_AI(2),'kx','linewidth',3)    
    plot(-L_DI(1),L_DI(2),'co','linewidth',2)    
    
    plot(-Px, Py,'rx')
 
    % Visualize Line of Instant Center
    line([-L_D(1) -L_DI(1)], [L_D(2) L_DI(2)],'color','y','linewidth',1)
    line([-L_A(1) -L_AI(1)], [L_A(2) L_AI(2)],'color','y','linewidth',1)
    
    line([origin(1) -L_A(1)], [origin(2) L_A(2)],'linewidth',2.5)
    line([-L_A(1) -L_B(1)], [L_A(2) L_B(2)],'linewidth',2.5)
    line([-L_D(1) -L_C(1)], [L_D(2) L_C(2)], 'linewidth',2.5)
    line([-L_A(1) -P(1)], [L_A(2) P(2)],'linewidth',2.5)
    line([-L_B(1) -P(1)], [L_B(2) P(2)],'linewidth',2.5)
    
    %Visualize R-vector
    %line( [-L_AI(1), -L_AI(1)-R_load(1)], [L_AI(2),L_AI(2)+R_load(2)], 'color','k');
   
    
    %Scaled Velocity
    vPs = vP/10;
    quiver( -P(1), P(2), -vPs(1), vPs(2),'g');    
 
    % R vector 
    quiver( -L_AI(1), L_AI(2), -R_load(1), R_load(2),'k');
    % Torque Visualization
    quiver( -P(1), P(2), 0, (T_Load(3)/10),'m');
 
    % Force Visualization
    quiver( -P(1), P(2), 0, F_load(2)/75,'k');
    
    title('DinoBank','fontsize',15)
    xlabel('x','fontsize',15)
    ylabel('y','fontsize',15)
    %axis equal
    axis([xaxis_min, xaxis_max, yaxis_min, yaxis_max])
    %pause(0.01)
    hold off
    frame = getframe;
    writeVideo(writerObj,frame);
 
end
close(writerObj); %Save video
 
hold on
h1 = figure(1)
saveas(h1, '4Bar_path_plot.pdf');
plot(-Px,Py,'rx')
hold off
h2 = figure(2)
plot(radtodeg(q2_i), fliplr(vP_i))
title('velP in x direction')
xlabel('theta 2 deg')
ylabel('vel P m/s')
saveas(h2, 'xVelofP');
h3 = figure(3)
plot(radtodeg(q2_i), T_Load_i);
xlabel('theta 2 deg');
ylabel('Torque on theta 3');
saveas(h3, 'torqueOn3.pdf');
h4 = figure(4)
 
MA = w2*(1./w3_i);
plot(radtodeg(q2_i), MA);
xlabel('theta 2 deg');
ylabel('Mechanical Advantage w2/w3');
saveas(h4, 'Ma.pdf');
 
h5 = figure(5)
hold on
plot(radtodeg(q2_i), T_Load_i./MA);
plot(radtodeg(q2_i), T_op_Nm*ones(1,size(q2_i,2)),'r');
plot(radtodeg(q2_i), -T_op_Nm*ones(1,size(q2_i,2)),'r');
title('Torque Seen by Motor (Nm)')
xlabel('theta 2 deg');
ylabel('Torque Nm');
legend ('Torque Required', 'Operating Torque Available');
saveas(h5, 'TorqueAtMotor.pdf');