How to take 2 sets of coordinates and altitude and calculate angle for Servos