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

Example 16 with Angle

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

the class TestStatTracker method testTicksPerSecond.

public void testTicksPerSecond() {
    Pathfinder pathfinder = Pathfinder.newSimulatedPathfinder(0.01).setSpeed(0.75).setTolerance(2).setAngleTolerance(Angle.fixedDeg(10));
    pathfinder.loadPlugin(new StatTracker());
    pathfinder.goTo(new PointXYZ(10, 10, 0));
    pathfinder.splineTo(new PointXYZ(10, 10, 90), new PointXYZ(12, 11, 45), new PointXYZ(13, 15, 180));
    ElapsedTimer timer = new ElapsedTimer(true);
    SimulatedOdometry odometry = (SimulatedOdometry) pathfinder.getOdometry();
    AtomicInteger i = new AtomicInteger(0);
    try {
        while (timer.elapsedSeconds() < 5) {
            if (Math.random() > 0.5)
                Thread.sleep(2);
            pathfinder.tick();
            Translation translation = pathfinder.getTranslation();
            odometry.setRawPosition(odometry.getRawPosition().add(new PointXYZ(translation.vx() / 50, translation.vy() / 50, Angle.fixedRad(translation.vz() / 50))));
        }
    } catch (Exception ignored) {
    }
    pathfinder.tick();
    System.out.println("tps: " + pathfinder.ticksPerSecond());
    System.out.println("ticks: " + pathfinder.getData("pf_ticks"));
    System.out.println("position: " + pathfinder.getPosition());
    System.out.println("condition met: " + i.get());
    System.out.println("total PointXY: " + PointXY.COUNT);
    System.out.println("total PointXYZ: " + PointXYZ.COUNT);
    System.out.println("total Angle: " + Angle.COUNT);
}

Example 17 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 18 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 19 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 20 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);
}