public static double setDistanceToPoint(Pose actualPose, double destX, double destY) {
   destX = -destX;
   double d =
       Math.sqrt(
           (Math.pow((destX - actualPose.getLocation().getX()), 2.0d)
               + (Math.pow((destY - actualPose.getLocation().getY()), 2.0d))));
   return d;
 }
 // ---- obie funkcje przyjmuja wartoœci dodatnie, zarówno dla x jak i y --//
 // zwraca wartoœæ o jak¹ ma obróciæ siê robot aby staæ przodem do docelowych wspó³rzênych x,y
 public static double setDirection(Pose actualPose, double destX, double destY) {
   double newAngle =
       (Math.atan2(
           actualPose.getLocation().getY() - destY, actualPose.getLocation().getX() + destX));
   System.out.println(
       -(Math.toDegrees(angleDiff(Math.toRadians(actualPose.getHeading()), newAngle))));
   return -(Math.toDegrees(angleDiff(Math.toRadians(actualPose.getHeading()), newAngle)));
 } // u¿ycie:
 /**
  * Returns a waypoint of the current location.
  *
  * @return Waypoint, current location
  */
 @Override
 public Waypoint getLocation() {
   PoseProvider provider = nav.getPoseProvider();
   Pose pose = provider.getPose();
   return (Waypoint) pose.getLocation();
 }