Examples with MultiSplineBuilder me.wobblyyyy.pathfinder2.trajectory.spline.MultiSplineBuilder used on opensource projects

Example 1 with MultiSplineBuilder

use of me.wobblyyyy.pathfinder2.trajectory.spline.MultiSplineBuilder in project Pathfinder2 by Wobblyyyy.

the class TestSimulatedChassis method testSplineTicksPerSecond.

@Test
public void testSplineTicksPerSecond() {
    // if your computer is fast enough, this should be 500 ticks
    // per second. if it's not... oh well.
    MultiSplineBuilder builder = new MultiSplineBuilder().setDefaultStep(0.5).setDefaultSpeed(0.5).setDefaultTolerance(0.5).setDefaultInterpolationMode(InterpolationMode.DEFAULT).setDefaultAngleTolerance(Angle.fromDeg(5));
    for (int i = 0; i < 10; i++) builder.add(i * 2, Math.pow(i, 2), Angle.fromDeg(0), Math.pow((i + 1) / 10d, 2), 0.5, 2, Angle.fromDeg(5));
    pathfinder.followTrajectory(builder.build());
    pathfinder.tickUntil(500);
}

Example 2 with MultiSplineBuilder

use of me.wobblyyyy.pathfinder2.trajectory.spline.MultiSplineBuilder in project Pathfinder2 by Wobblyyyy.

the class TestSimulatedChassis method testNonMonotonicMultiSplineTrajectory.

@Test
public void testNonMonotonicMultiSplineTrajectory() {
    MultiSplineBuilder builder = new MultiSplineBuilder().setDefaultStep(0.05).setDefaultSpeed(0.5).setDefaultTolerance(0.5).setDefaultAngleTolerance(Angle.fromDeg(5)).add(0, 0, Angle.fromDeg(0), 0.1).add(2, 3, Angle.fromDeg(0), 0.1).add(4, 6, Angle.fromDeg(0), 0.1).add(6, 9, Angle.fromDeg(0), 0.1).add(8, 6, Angle.fromDeg(0), 0.1).add(10, 3, Angle.fromDeg(0), 0.1).add(12, 6, Angle.fromDeg(0), 0.1).add(14, 9, Angle.fromDeg(0), 0.1).add(16, 6, Angle.fromDeg(0), 0.1).add(18, 3, Angle.fromDeg(0), 0.1).add(20, 0, Angle.fromDeg(0), 0.5, 0.1, 2, Angle.fromDeg(5));
    pathfinder.followTrajectory(builder.build());
    pathfinder.tickUntil(500);
    assertPositionIs(new PointXYZ(20, 0, 0));
}

Example 3 with MultiSplineBuilder

use of me.wobblyyyy.pathfinder2.trajectory.spline.MultiSplineBuilder in project Pathfinder2 by Wobblyyyy.

the class TestSimulatedChassis method testMonotonicMultiSplineTrajectory.

@Test
public void testMonotonicMultiSplineTrajectory() {
    MultiSplineBuilder builder = new MultiSplineBuilder().setDefaultStep(0.05).setDefaultSpeed(0.5).setDefaultTolerance(0.5).setDefaultAngleTolerance(Angle.fromDeg(5)).add(0, 0, Angle.fromDeg(0), 0.1).add(2, 3, Angle.fromDeg(0), 0.1).add(4, 6, Angle.fromDeg(0), 0.1).add(6, 9, Angle.fromDeg(0), 0.5, 0.1, 2, Angle.fromDeg(5));
    pathfinder.followTrajectory(builder.build());
    pathfinder.tickUntil(500);
    assertPositionIs(new PointXYZ(6, 9, 0));
}

Example 4 with MultiSplineBuilder

use of me.wobblyyyy.pathfinder2.trajectory.spline.MultiSplineBuilder 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;
}