%% Robotic arm motion
%

L1=50;
L2=50;
T1=-25% -30 ... 60
T2=50% 0 ... 150
xe=L1*cosd(T1);
ye=L1*sind(T1);
xh=L1*cosd(T1)+L2*cosd(T1+T2);
yh=L1*sind(T1)+L2*sind(T1+T2);
figure;
set(gca,'Visible','off')
hold on;
plot([-50 110],[0 0],"-k")
plot([0 0],[-50 110],"-k")
text(105,5,"x")
text(1,109,"y")
plot([0 xe xh],[0 ye yh],"-b","Marker","o")
axis([-50 110 -50 110])
hold off

% We create a function that displays the robotic arm based on information about the angles and the lengths of the two links.

figure;
plotarm(T1,T2,L1,L2);

%
% What is the area covered by the endpoint of the second link (hand)?

L1=50;
L2=50;
figure
pause on
for T1=-30:10:60
    for T2=0:10:150
        plotarm(T1,T2,L1,L2)
        pause(0.05)
    end
    pause(0.2)
end

%
% Homework 1
% Create a script that draws the boundary of the area covered by the hand of the robotic arm.





%
% How can we determine the angles T1 and T2 knowing the position of the endpoint of the second link (hand)?
%

xh=15
yh=50
Th=atand(yh/xh);
Lh=sqrt(xh*xh+yh*yh);
A1=acosd((L1*L1+Lh*Lh-L2*L2)/2/L1/Lh);
T1=Th-A1;
xe=L1*cosd(T1);
ye=L1*sind(T1);
T2=acosd((xh-xe)/L2)-T1;
figure;
plotarm(T1,T2,L1,L2);

%
%% Create a script that draws the robotic arm for two different hand positions (xh0,yh0 and xh1,yh1)

L1=50;
L2=50;
figure;
xh0=15;
yh0=50;
[T10,T20]=getT(xh0,yh0,L1,L2);
plotarm(T10,T20,L1,L2);
xh1=90;
yh1=0;
[T11,T21]=getT(xh1,yh1,L1,L2);
plotarm(T11,T21,L1,L2);

%
%% Define a linear trajectory between the two hand positions with 20 points

x=linspace(xh0,xh1,20);
y=linspace(yh0,yh1,20);
figure
hold off
for i=1:length(x)
    [T1i,T2i]=getT(x(i),y(i),L1,L2);
    plotarm(T1i,T2i,L1,L2);
    pause(0.2)
end

%
%% Homework 2
% Define a non-linear (e.g. arc) trajectory between the two hand positions

