Adjust algorithm for robot's angle in world space