% Program 2.1
% R-RRT
% Position analysis
clear      % clears all variables from the workspace
clc        % clears the command window and homes the cursor
close all  % closes all the open figure windows
% Input data 
AB=0.5;
BC=1;
phi = pi/4;

% Position of joint A (origin)
xA = 0; yA = 0;

% Position of joint B - position of the driver link
xB = AB*cos(phi);
yB = AB*sin(phi);

% Position of joint C 
yC = 0;
% Distance formula: BC=constant
eqnC = '( xB - xCsol )^2 + ( yB - yC )^2 = BC^2';
% Solve the above equation
solC = solve(eqnC, 'xCsol');
% solve symbolic solution of algebraic equations
% Two solutions for xC - vector form
xC1=eval(solC(1)); % first  component of the vector solC
xC2=eval(solC(2)); % second component of the vector solC
% eval executes string as an expression or statement
% Select the correct position for C for the given input angle
if xC1 > xB xC = xC1; else xC = xC2; end 
% if conditionally executes statements

% Angle of the link 2 with the horizontal
phi2 = atan((yB-yC)/(xB-xC));

fprintf('Results \n'); fprintf(' \n');
% Print the coordinates of B
fprintf('xB =  %g (m)\n', xB);
fprintf('yB =  %g (m)\n', yB);
% Print the coordinates of C
fprintf('xC =  %g (m)\n', xC);
fprintf('yC =  %g (m)\n', yC);
% Print the angle phi2
fprintf('phi2 = %g (degrees) \n', phi2*180/pi);

% Graphic of the mechanism
plot( [xA,xB],[yA,yB],'r-o', [xB,xC],[yB,yC],'b-o' ),...    
xlabel('x (m)'),... 
ylabel('y (m)'),... 
title('positions for \phi = 45 (deg)'),... 
text(xA,yA,'  A'),... 
text(xB,yB,'  B'),... 
text(xC,yC,'  C'),... 
axis([-0.2 1.4 -0.2 1.4]),...
grid
% the commas and ellipses (...) after the commands were used 
% to execute the commands together