Examples with InvalidToleranceException me.wobblyyyy.pathfinder2.exceptions.InvalidToleranceException used on opensource projects

Example 1 with InvalidToleranceException

use of me.wobblyyyy.pathfinder2.exceptions.InvalidToleranceException in project Pathfinder2 by Wobblyyyy.

the class AdvancedSplineTrajectoryBuilder method build.

public AdvancedSplineTrajectory build() {
    boolean invalidStep = step == Double.MAX_VALUE;
    boolean invalidSpeed = speed == Double.MAX_VALUE;
    boolean invalidTolerance = tolerance == Double.MAX_VALUE;
    boolean invalidAngleTolerance = angleTolerance == null;
    DEFAULT_LOGGER.accept("--- BUILDING SPLINE ---");
    DEFAULT_LOGGER.accept(StringUtils.format("invalid step: %s\n" + "invalid speed: %s\n" + "invalid tolerance: %s\n" + "invalid angle tolerance: %s", invalidStep, invalidSpeed, invalidTolerance, invalidAngleTolerance));
    if (invalidStep && invalidSpeed && invalidTolerance && invalidAngleTolerance)
        throw new IllegalArgumentException("Did not set a step, speed, tolerance, and angle tolerance " + "value! You need to use setStep(), setSpeed(), " + "setTolerance(), and setAngleTolerance() before " + "calling the build() method.");
    if (invalidStep)
        throw new IllegalArgumentException("Did not set a step value - use setStep().");
    if (invalidSpeed)
        throw new InvalidSpeedException("Did not set a speed - use setSpeed().");
    if (invalidTolerance)
        throw new InvalidToleranceException("Did not set a tolerance - use setTolerance().");
    if (invalidAngleTolerance)
        throw new NullAngleException("Null angle tolerance while creating an " + "AdvancedSplineTrajectory.");
    int size = xValues.size();
    Double[] xBoxed = new Double[size];
    Double[] yBoxed = new Double[size];
    Double[] speedBoxed = new Double[size];
    Angle[] z = new Angle[size];
    xValues.toArray(xBoxed);
    yValues.toArray(yBoxed);
    angleTargets.toArray(z);
    speeds.toArray(speedBoxed);
    double[] x = new double[size];
    double[] y = new double[size];
    double[] speed = new double[size];
    boolean sameSpeedValue = true;
    int xDuplicates = 0;
    int yDuplicates = 0;
    for (int i = 0; i < xBoxed.length; i++) {
        x[i] = xBoxed[i];
        y[i] = yBoxed[i];
        speed[i] = speedBoxed[i];
        if (i != 0) {
            if (speed[i] != speed[i - 1])
                sameSpeedValue = false;
            // ensure adjacent X values are unique
            if (x[i] == x[i - 1])
                x[i] += Core.advancedSplineTrajectoryDuplicateOffset * ((xDuplicates++) + 1);
            // ensure adjacent Y values are unique
            if (y[i] == y[i - 1])
                y[i] += Core.advancedSplineTrajectoryDuplicateOffset * ((yDuplicates++) + 1);
        }
    }
    DEFAULT_LOGGER.accept(StringUtils.format("boxed x: %s\n" + "boxed y: %s\n" + "boxed z: %s\n" + "boxed speed: %s\n" + "unboxed x: %s\n" + "unboxed y: %s\n" + "unboxed speed: %s", Arrays.toString(xBoxed), Arrays.toString(yBoxed), Arrays.toString(z), Arrays.toString(speedBoxed), Arrays.toString(x), Arrays.toString(y), Arrays.toString(speed)));
    // add support for different types of spline interpolation!
    // this is a really bad way to implement support for multiple
    // types of spline interpolation, but... oh well.
    Spline spline;
    switch(interpolationMode) {
        case DEFAULT:
            spline = new MonotoneCubicSpline(x, y);
            break;
        case CUBIC:
            spline = new ApacheSpline(Interpolator.CUBIC, x, y);
            break;
        case AKIMA:
            spline = new ApacheSpline(Interpolator.AKIMA, x, y);
            break;
        case CUSTOM:
            if (customSplineGenerator != null)
                spline = customSplineGenerator.apply(xBoxed, yBoxed);
            else
                throw new NullPointerException("Tried to use custom " + "spline generator without having set it first: " + "use setCustomSplineGenerator() to do so. The " + "function you pass in should accept two arrays " + "of Double values (x and y).");
            break;
        default:
            throw new RuntimeException("How did you even get here?");
    }
    AngleSpline angleSpline = new AngleSpline(x, z);
    /*
        Spline speedSpline;
        if (sameSpeedValue) speedSpline =
            new LinearSpline(
                new SlopeIntercept(0, speed[0])
            ); else speedSpline = new MonotoneCubicSpline(x, speed);
        */
    Spline speedSpline = new MonotoneCubicSpline(x, speed);
    Logger.debug(AdvancedSplineTrajectoryBuilder.class, "spline: <%s> angleSpline: <%s> speedSpline: <%s> step: <%s> " + "tolerance: <%s> angleTolerance: <%s>", spline, angleSpline, speedSpline, step, tolerance, angleTolerance);
    return new AdvancedSplineTrajectory(spline, angleSpline, speedSpline, step, tolerance, angleTolerance);
}

Example 2 with InvalidToleranceException

use of me.wobblyyyy.pathfinder2.exceptions.InvalidToleranceException in project Pathfinder2 by Wobblyyyy.

the class Pathfinder method splineTo.

/**
 * Create a spline trajectory to a certain target point, and then follow
 * that aforementioned trajectory.
 *
 * <p>
 * If this method is called on a set of points with non-monotonic Y
 * values, this will instead invoke
 * {@link #multiSplineTo(double, double, Angle, PointXYZ...)}, which
 * supports non-monotonic Y values.
 * </p>
 *
 * @param speed          the speed at which the robot should move. This
 *                       is a constant value.
 * @param tolerance      the tolerance used for determining whether the
 *                       robot is at the target point.
 * @param angleTolerance same thing as {@code tolerance}, but for the
 *                       robot's angle.
 * @param points         a set of control points for the spline. This
 *                       will automatically insert the robot's current
 *                       position into this array. This array must have
 *                       AT LEAST two points.
 * @return {@code this}, used for method chaining.
 */
public Pathfinder splineTo(double speed, double tolerance, Angle angleTolerance, PointXYZ... points) {
    if (points.length < 2)
        throw new IllegalArgumentException("At least two control points are required to use the " + "splineTo method.");
    InvalidSpeedException.throwIfInvalid("Invalid speed value provided! Speed must be between 0 and 1.", speed);
    InvalidToleranceException.throwIfInvalid("Invalid tolerance! Tolerance must be a positive number.", tolerance);
    if (angleTolerance.deg() < 0)
        throw new InvalidToleranceException("Invalid angle tolerance! " + "Angle tolerance must be greater than 0 degrees.");
    NotNull.throwExceptionIfNull("One or more points provided to splineTo was null!", (Object[]) points);
    // non-monotonic Y values means we need to use a multi spline instead
    if (!Spline.areMonotonicY(points))
        return multiSplineTo(speed, tolerance, angleTolerance, points);
    // length is the total distance of the spline (NOT all control points)
    double length = PointXY.distance(points[0], points[points.length - 1]);
    // step should be relatively small - by default, it's 1/20th of
    // the spline's length.
    double step = length / Core.pathfinderStepDivisor;
    AdvancedSplineTrajectoryBuilder builder = new AdvancedSplineTrajectoryBuilder().setSpeed(speed).setTolerance(tolerance).setAngleTolerance(angleTolerance).setStep(step);
    PointXYZ robotPosition = getPosition();
    // to the trajectory
    if (!robotPosition.equals(points[0]))
        builder.add(robotPosition);
    PointXYZ lastPoint = points[0];
    for (int i = 1; i < points.length; i++) {
        PointXYZ point = points[i];
        if (point == null)
            throw new NullPointException("Cannot use the splineTo method with a null " + "control point!");
        if (!point.equals(lastPoint)) {
            builder.add(point);
            lastPoint = point;
        } else {
            throw new SplineException("There were duplicate adjacent points in the set " + "of control points! This means there's the same " + "point, twice in a row. The points were: " + Arrays.toString(points));
        }
    }
    Trajectory trajectory = builder.build();
    return followTrajectory(trajectory);
}

Example 3 with InvalidToleranceException

use of me.wobblyyyy.pathfinder2.exceptions.InvalidToleranceException in project Pathfinder2 by Wobblyyyy.

the class Pathfinder method multiSplineTo.

/**
 * Use a {@link MultiSplineBuilder} to construct a spline trajectory.
 *
 * @param speed          the speed at which the robot should move. This
 *                       is a constant value.
 * @param tolerance      the tolerance used for determining whether the
 *                       robot is at the target point.
 * @param angleTolerance same thing as {@code tolerance}, but for the
 *                       robot's angle.
 * @param points         a set of control points for the spline. This
 *                       will automatically insert the robot's current
 *                       position into this array. This array must have
 *                       AT LEAST two points.
 * @return {@code this}, used for method chaining.
 */
public Pathfinder multiSplineTo(double speed, double tolerance, Angle angleTolerance, PointXYZ... points) {
    if (points.length < 2)
        throw new IllegalArgumentException("At least two control points are required to use the " + "splineTo method.");
    checkForMissingDefaultValues();
    InvalidSpeedException.throwIfInvalid("Invalid speed value provided! Speed must be between 0 and 1.", speed);
    InvalidToleranceException.throwIfInvalid("Invalid tolerance! Tolerance must be a positive number.", tolerance);
    if (angleTolerance.deg() < 0)
        throw new InvalidToleranceException("Invalid angle tolerance! " + "Angle tolerance must be greater than 0 degrees.");
    if (!Spline.areMonotonicX(points))
        throw new SplineException("Cannot create a spline with non-" + "monotonic X values! X values can only be either " + "increasing or decreasing, but not a combination of both.");
    double totalDistanceX = points[points.length - 1].distanceX(points[0]);
    double step = totalDistanceX / (points.length * Core.pathfinderSplineStepCoefficient);
    MultiSplineBuilder builder = new MultiSplineBuilder().setDefaultSpeed(speed).setDefaultTolerance(tolerance).setDefaultAngleTolerance(angleTolerance).setDefaultStep(step);
    for (PointXYZ point : points) builder.add(point, speed, step);
    Trajectory trajectory = builder.build();
    followTrajectory(trajectory);
    return this;
}

Example 4 with InvalidToleranceException

use of me.wobblyyyy.pathfinder2.exceptions.InvalidToleranceException in project Pathfinder2 by Wobblyyyy.

the class PointXY method isNear.

/**
 * Are two points close to each other?
 *
 * @param a         one of the two points.
 * @param b         one of the two points.
 * @param tolerance the maximum distance value.
 * @return if the `distance(a, b)` method call returns a value less than
 * or equal to the {@code tolerance}, this method will return true.
 * Else, this method will return false.
 */
public static boolean isNear(PointXY a, PointXY b, double tolerance) {
    ValidationUtils.validate(a, "a");
    ValidationUtils.validate(b, "b");
    InvalidToleranceException.throwIfInvalid("Invalid tolerance!", tolerance);
    if (tolerance < 0)
        throw new InvalidToleranceException(StringUtils.format("Cannot have a tolerance below 0! Tolerance was: <%s>", tolerance));
    if (distanceX(a, b) > tolerance || distanceY(a, b) > tolerance)
        return false;
    return Math.abs(distance(a, b)) <= Math.abs(tolerance);
}