Examples with Angle me.wobblyyyy.pathfinder2.geometry.Angle used on opensource projects

Example 1 with Angle

use of me.wobblyyyy.pathfinder2.geometry.Angle in project Pathfinder2 by Wobblyyyy.

the class TestSimulatedChassis method assertPositionIs.

private void assertPositionIs(PointXYZ target) {
    PointXYZ position = pathfinder.getPosition();
    double distance = position.distance(target);
    Angle angleDistance = Angle.fromDeg(Math.abs(Angle.minimumDelta(position.z(), target.z())));
    Assertions.assertTrue(distance <= DEFAULT_TOLERANCE, StringUtils.format("Could not assert position! Expected <%s> but got " + "<%s> instead. Distance <%s> was greater " + "than maximum distance of <%s>!", target, position, distance, DEFAULT_TOLERANCE));
    Assertions.assertTrue(angleDistance.deg() <= 5, StringUtils.format("Could not assert angle! Expected <%s> but got " + "<%s> instead. Distance <%s> was greater " + "than maximum distance of <%s>!", target.z(), position.z(), angleDistance, "5 deg"));
}

Example 2 with Angle

use of me.wobblyyyy.pathfinder2.geometry.Angle in project Pathfinder2 by Wobblyyyy.

the class TestControlledTrajectory method testSingleController.

private void testSingleController(Controller controller, PointXYZ target, double tolerance, Angle angleTolerance) {
    PointXYZ origin = pathfinder.getPosition();
    Trajectory toTarget = new ControlledTrajectory(target, controller, tolerance, angleTolerance);
    Trajectory toOrigin = new ControlledTrajectory(origin, controller, tolerance, angleTolerance);
    follow(toTarget, target);
    follow(toOrigin, origin);
}

Example 3 with Angle

use of me.wobblyyyy.pathfinder2.geometry.Angle in project Pathfinder2 by Wobblyyyy.

the class TestSequentialLinearTrajectory method testTurnTo.

private void testTurnTo(Angle... angles) {
    for (Angle angle : angles) {
        PointXYZ point = new PointXYZ().addZ(angle);
        testTrajectoryTo(point);
    }
}

Example 4 with Angle

use of me.wobblyyyy.pathfinder2.geometry.Angle in project Pathfinder2 by Wobblyyyy.

the class MultiTargetTrajectory method nextMarker.

@Override
public PointXYZ nextMarker(PointXYZ current) {
    // set up some values
    if (!hasRan) {
        lastStartX = current.x();
        lastStartY = current.y();
        lastStartZ = current.z();
        PointXYZ next = targets[index].target();
        requiredTranslationX = next.x() - current.x();
        requiredTranslationY = next.y() - current.y();
        requiredTranslationZ = next.z().subtract(current.z()).fix().deg();
        hasRan = true;
    }
    TrajectoryTarget target = targets[index];
    // current values
    double currentX = current.x();
    double currentY = current.y();
    Angle currentAngle = current.z();
    // sort of "delta" values - difference between current and start
    double translationX = currentX - lastStartX;
    double translationY = currentY - lastStartY;
    double translationZ = currentAngle.subtract(lastStartZ).fix().deg();
    boolean hasCompletedX = Magnitude.higherMagnitude(translationX, requiredTranslationX);
    boolean hasCompletedY = Magnitude.higherMagnitude(translationY, requiredTranslationY);
    boolean hasCompletedZ = Magnitude.higherMagnitude(translationZ, requiredTranslationZ);
    if (hasCompletedX && hasCompletedY && hasCompletedZ) {
        // to handle tolerance values
        if (target.precision() == TargetPrecision.PRECISE) {
            PointXYZ targetPoint = target.target();
            double distance = current.absDistance(targetPoint);
            Angle angleDistance = Angle.angleDelta(current.z(), targetPoint.z());
            boolean validDistance = distance < target.tolerance();
            boolean validAngle = angleDistance.lessThanOrEqualTo(target.angleTolerance());
            if (!(validDistance && validAngle)) {
                return targetPoint;
            }
        }
        // care about tolerance
        if (index == targets.length - 1) {
            // if it's the last target point, the entire trajectory is done
            hasFinished = true;
            return current;
        } else {
            lastStartX = currentX;
            lastStartY = currentY;
            lastStartZ = currentAngle;
            index++;
            PointXYZ next = targets[index].target();
            requiredTranslationX = next.x() - currentX;
            requiredTranslationY = next.y() - currentY;
            requiredTranslationZ = next.z().subtract(currentAngle).fix().deg();
        }
    }
    return targets[index].target();
}

Example 5 with Angle

use of me.wobblyyyy.pathfinder2.geometry.Angle 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);
}