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();
}
