getAngle.m

function [q1,q2] = getAngle(x_wanted,y_wanted, l1, l2)
%UNTITLED2 Summary of this function goes here
%   Detailed explanation goes here
if nargin==2
    l1=0.335;
    l2=0.263;

end
d_squared=x_wanted^2+y_wanted^2;

alpha=acos((l1^2+d_squared-l2^2)/(2*l1*sqrt(d_squared)));

if l1+l2-sqrt(d_squared)<0
    disp("position can not be reached, arm is too short")
end
beta = acos((l1^2+l2^2-d_squared)/(2*l1*l2));
if beta<pi-2.45
    disp("position can not be reached, elbow angle would be too tight")
end
gamma = atan2(y_wanted,x_wanted);

q1=gamma-alpha+pi/2;
if q1 >2*pi/3
    disp("position can not be reached, unable to lift upper arm so high")
elseif q1<0
    disp("position can not be reached, body is in the way")
end
q2=pi-beta;

% l1*sin(q1)+l2*sin(q1+q2)-x_wanted
% l1*cos(q1)+l2*cos(q1+q2)-y_wanted

end