function [psi, r] = getPsi(RobPosX, RobPosY, psiX, psiY)

%TODO Die Ausrichtung des Roboters ist nicht in den Paramtern, oder?
%Wie soll diese mit einfließen?

%Abstand berechnen
abstandX = psiX - RobPosX;
abstandY = psiY - RobPosY;
r = sqrt(abstandX^2 + abstandY^2)

%Die Winkel berechnen, von der X-Achse gesehen 
sinAlphaPsi = psiY / sqrt(psiX^2 + psiY^2)
winkelPsi = asin(sinAlphaPsi)

psi = winkelPsi

end

